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I 



INTRODUCTION 



A. TARGET TRACKING 

The detection, location, and tracking of targets plays a 
critical role in many aspects of military operations. The 
ability to accurately track and predict the movement of 
aircraft, anti-ship missiles, and torpedoes in military 
operations can not be overstated. This report investigates 
the use of multiple sensors and Kalman filtering in Fusion 
Tracking. 

The Kalman filter has been in use for over three decades 
in aircraft avionics, radar, sonar, and multiple control 
problems. This investigation covers a Kalman filter in 
tracking a target by bearing observations and an additional 
problem using bearing, range, and frequency observations. 
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II. THE KALMAN FILTER 



The Kalman filter estimates the state of a linear and 
time-invariant (LTI) system given a set of known inputs to the 
system and a set of measurements. 



A. DEFINITION OF TERMS 

The terms used in this chapter and Appendix B, are defined 
in Table (2.1). Kalman filter system dynamics utilize state 
space representation, therefore matrix dimensions are given. 
TABLE 2.1: Definition of Terms 



Dimension Code Symbol 



Error covariance matrix: 
Estimate error: 

Expected value of error: 
Identity matrix: 

Kalman gain matrix: 
Measurement matrix: 
Observation : 

Observation noise: 
Residual : 

State estimate: 

State excitation noise: 
State transition matrix: 
System state: 

Transpose: 



n 


X 


n 


P 




n 


X 


1 






n 


X 


1 






n 


X 


n 


eye (n) 


I 








G(k) 


■G, 
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X 


1 


H 


H 








theta (k) 


z l 


1 


X 


1 


R 


v, 


1 


X 


1 




r. 
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X 
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XKK 
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w i 
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X 
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Phi 
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X 


1 


X 


x i 








X' 


X 



Appendix A defines the terms used in the Kalman filter 
designed around the torpedo tracking problem. This program is 
discussed further in Chapter IV. 
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B. EQUATIONS AND INITIALIZATION 

The background given here coincides with that of [Ref 1] . 
The equations for the filter used in this program are listed 
below. 



P k\k-^ T ^ P k\k-x pT+ ^ - 1 


(2.1) 


D k\k = -PjtNjc-i 


(2.2) 


P k*l\k = ^ >P k\k^ >T+ ^k 


(2.3) 


~ X k \ k - 1 + G k [ z k ~ 2 k ^ k . 1 ] 


(2.4) 


/V . A 

= Y-XjcVc 


(2.5) 


Z k\k-1 = H^kXk-l 


(2.6) 



Terms with a single time subscript (i.e., x k ) refer to the 
value of the term at that time. Those terms with dual 

subscripts (i.e., P (k+nk) ) refer to the value of the term at a 
future time given the present states. 

The Kalman filter requires initialization and the proper 
choice of initialization is important. The value of is 

initially set to zero and the following steps are then 
evaluated in the recursion algorithm. 

• Use Equation 2.1 to calculate the Kalman gain. 

• Use Equation 2.2 and 2.3 to evaluate the values of the 
covariance of estimate error matrix for the next 
iteration. 

• Use Equation 2.4 to find the state estimates. 

• Use Equation 2.5, given the current state estimates to 
project ahead for use in the next iteration. 
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In the program, the bearing theta is simulated and sampled by 
the filter as if they were taken from a sensor. 
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Ill 



THE MODEL 



In this chapter, we introduce the mathematical and 
physical model for our first tracking problem. We start by 
presenting the physical relationship between the target and 
the observer. After the track has been generated, we use the 
Kalman filter to verify that the mathematical model is 
tracking the physical model. 



A. PHYSICAL SYSTEMS 



The first system used in this investigation consists of a 
single observer and a single target. For the generation of 
the target's track, a two-dimensional Cartesian coordinate 
system with the observer at the origin is used. When 
referring to Figure 3.1, this x-y grid can be thought of as 
looking down on a north-south/east-west grid. The target is 
free to move, unrestricted, throughout the coordinate space 
while the observer is stationary and remains at the origin. 

The information received by the observer consists of 
bearing to the target. Therefore, the state space estimate 
appears as: 



X = 



e 

6 

0 



(3.1) 
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Figure 3.1: First Physical System 



These bearings are to be consistent with measurements supplied 
by a phased array radar, in receive only mode, and most types 
of infra-red scanners used today. The measurements are taken 
from the radar 100 times per second, while the infra-red is 
sampled every second. 



B. MODEL TEST 

It is critical that the recursive program receive its 
initial states and observations in the proper order. 
Therefore, we will examine the Kalman gains for a 
verification. The MATLAB programs MISSILE1.M and MISSILE2 . M 
used are found in Appendix B. For the purpose of 
verification, we will assume no noise with the covariance of 
measurement noise and covariance of excitation equal to zero. 
If the filter is working properly, the Kalman gains for the 
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first sample should have the position vector G(l,l) equal to 
one. The velocity and acceleration vectors, G(2,l) and G(3,l) 
respectively, should equal zero (see Figures 3.9 through 
3.11). At the second sample, G(l,2) is given by i?(2) = z( 2) , 
G ( 2 , 2 ) is given by (l/T) [z(2)-z(l)] , and G(3,n) should remain 
zero. For the third sample, G(3,3) should take on a value 
given by 

(l/T 2 ) { [z (3) -z(2) ] - [z(2) -z(l)]}. (3.2) 



For this simulation the target has v t = 1 km/sec as shown 
in Figure 3.3. Figures 3.4 and 3.5 show the distance traveled 
and change in bearing over one second. The figure showing the 
change in bearing might be deceptive, as it appears to be a 
straight line. In fact it is a curved line that reaches its 
maximum rate of change when the observer is perpendicular to 
the target's path. Figures 3.6 through 3.8 show the state 
estimates of the Kalman filter, that gives us a comparison to 
the actual change in bearing. 

Figures 3.9 through 3.11 are the Kalman gains. However, 
the covariance of measurement noise is 0.001. This avoids a 
divide by zero in the Kalman gain equations. This will have 
a minimal effect on the results. 
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Figure 3.2: Geometry of the target 




O 0.1 0.2 0.3 0.4- 0.5 ~ 0.6 
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Figure 3.3: Targets track for one second 



Bearing to the target 




(S-c) 



Figure 3.4: Observed change in bearing 
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Figure 3.5: State estimate of bearing 



Estimate of Angular rate 
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Figure 3.6: State estimate of change in rate 




Figure 3.7: 



State estimate of angular acceleration 
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Figure 3.8: Kalman filter gain vector G(l,k) 




Figure 3.9: Kalman filter gain vector G(2,k) 




(S«e) 

Figure 3.10: Kalman filter gain vector G(3,k) 
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IV. THE KALMAN FILTER WITH MANEUVER DETECTION 



A. PROGRAMS 

This chapter is devoted to an explanation of the 
components and logic used for the programs in Appendix C. 
TORPMAN . FOR uses the combined work from [Ref 1] and Raymond M. 
Alfaro. In order to run the program, follow this procedure. 

• Run TORPSCUR.M for the s-curve or TRACKS IM. M for the 
radial track as in [Ref 1]. 

• GENFILES.M converts the track to a data format. 

• POSCONV . FOR converts the MATLAB (ASCII) to FORTRAN 
(keyport) input. 

• Run OBSDATA. FOR to covert position, velocity, 
acceleration, time, and roll data files to a single 
observation file OBSERVAT.DAT . 

• TORPMAN. FOR is the Kalman filter and may be analyzed by 
using ANALYZE. M . 

B. STATE SPACE AND OBSERVATION MATRICES 

As stated previously, this problem utilizes the Cartesian 
coordinate system. The state space is made up of position, 
velocity, and acceleration in the x, y, and z vector space, 
where the dot denotes a time derivative. 

X = [x x x y y y z z z] T (4.1) 
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0 1 t 

0 0 1 



0 0 0 

0 0 0 
0 0 0 



0 0 0 

0 0 0 
0 0 0 



t 2 

00 0 It— 00 0 

2 

00 0 01 too 0 
000001000 

00 0 00 0 it — 

2 

00000001 t 
000000001 



( 4 . 2 ) 



The observation measurements for our system consists of 
position and velocity measurements with or without noise 
added. Observation measurements are defined in the following 
manner: 



x 

x 




z 

z 



We are assuming the acceleration information is not available. 
However, if it were possible to get the observation, the 
filter would take advantage of the opportunity to improve its 
estimate. We will also assume that some observation may have 
missing position and velocity data. This will be discussed in 
depth later in the chapter. 
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1. Physical Model 

For the system as shown in Figure 4.1, the Cartesian 
coordinate system is used. This implies an array of sensor 
generate the range measurements. Again the target is free to 
move throughout the coordinate space. However, the graphic 
information displays only X-Y coordinates. Frequency 
information is also utilized in the velocity estimate by 
estimating the turn counts. 




Figure 4.1: Second Physical System 



C. WEIGHTED COVARIANCE MATRIX 

The covariance of excitation error matrix, Q(k) , is 
considerably more complex. The matrix can be weighted or 
manipulated when a maneuver is detected. In this new 
arrangement, the x, y, and z coordinates are adjusted 
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independently of the others. This matrix was derived in [Ref 
2, pp. 36-42] and is given below. 



— w 0 0 0 0 0 0 0 

36 x 

t-4 

0 w x 0 0 0 0 0 0 

4 * 

0 0 1 2 w x 0 0 0 0 0 

o o o— w v 0 0 0 0 

36 y 

0 0 0 0 — w v 0 0 0 

4 y 

0 0 0 0 0 t 2 w y 0 0 

t 6 

0 0 0 0 0 0 — W z 0 

3 6 



0 



0 

0 



0 



0 



(4.4) 



0 



0 



0 0 0 0 
0 0 0 0 



0 0 
0 0 



t 4 

0 ±-w z 0 

0 0 1 2 w z 



The expression for distance is 




s 



where 

t = sample time interval, 
w = weighting factor, and 

s = distance travelled in one sample interval. 



(4.5) 



A good example of finding w is given in [Ref 1, pp. 22]. One 
should be reminded, when solving for w, it is the. worst case 
and a smaller weight is more robust. 



1. Error Ellipsoids 

Error ellipsoids are useful in visualizing the margins 
of error. The state value should lie within a certain region 
surrounding the estimate. This uncertainty is expressed in 
the covariance of error matrix P kxk . 

The position components submatrix of the error covariance 
matrix P kxk is defined as 



p = 


( p p \ 

*11 *14 


.1 


var x 


cov{x,y)\ 


o 

cm x; 

O 

v 


(4.6) 


xy 


P P 

\*41 *44/ 


\ 


,cov(x,y) 


var y ) 


°ly °y J 




The diagonal 


terms 




and P 44 


of the covariance matrix 


represents the variances of the estimate 


in the 


x and y 


positions respectfully. 


If 


is a nonnegative 


definite 



matrix and the random Gaussian noise processes w(k) and v(k) 
are independent, a surface of constant probability density can 
be produced. To elaborate on what is desired. Figure 4.2 is 
provided . 
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a. Matrix Method 

From the density function 

e~ xTpx , 

we specify the level surface about the origin by 



(4.7) 



x t P- 1 x = 1 . (4.8) 
From numerical analyses [Ref 3, pp. 164], there is a unique 
upper triangular matrix R such that 



P = R r R (4.9) 

Moreover, this matrix can be stably computed without pivoting. 

This result, however, is equivalent to stating that there is 

a corresponding unique triangular matrix L(=R T ) such that 

P = LL t (4.10) 



In terms of the matrix, our basic problem 4.8 becomes 

x t (LL t ) ~ 1 x = 1 (4.11) 



However, elementary matrix properties imply that 
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(4.12) 



x T (LL T ) - 1 = x t (L t ) ' 1 L" 1 x 

= (x t (L~ 1 ) t ) (L~ 1 x) 

= (L~\x) T (L _1 x) = jL^xllz 

where H 1 2 represents the usual Euclidean norm. But, this last 



equality implies that 

x T P' 1 x = 1 *==» ||L _ 1 x | 2 = 1 . (4.13) 

Therefore, the basic problem 4.10 is equivalent to 



|i _1 x| 2 = 1 . (4.14) 

This equation is solvable if and only if, 

L _1 x = y for some vector y such that ||y|| 2 = 1 (4.15) 

But, 



L' x x = y x = Ly . (4.16) 

Therefore, we conclude that the desired error ellipsoid is 
specified by 

{ x|x=Ly , ||y|| 2 =l ) (4.17) 

b. Reaction to a maneuver 

The effects of maneuver detection are seen in 
Figure 4.3. As the filter detects an acceleration in one 
direction, the uncertainty of the position increases in that 
dimension. The ellipsoids can be used to verify the filters 
ability to predict the movements of the target. The program 
ELLIPSE. M that generates the ellipsoids can be found in 
Appendix B. 

c. Initialization check 

The program S_ELLIPSE.M contained in Appendix C 
give us the same visual verification of the filter using the 
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3500 



Trajectory - S Curve 




X - Position (ft) 

Figure 4.3: The effect of maneuver detection on the 

covariance of error 

same idea. By using the error covariance matrix P we can 
obtain a visual insight into the performance of the filter. 
Figure 4.4 shows the exponential decay of the probability of 
error. 

D. THE OBSERVATION MATRIX - H 

The logic for the observation matrix starts with the 
manipulation of data in OBSDATA. FOR, in Appendix C. After 
receiving an observation, the data is manipulated to obtain a 
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r 



Trojectory - S Curve 




X - Position (ft) 

Figure 4.4: The correct response for the covariance of 

error ellipsoids 

state space vector with position, velocity, and acceleration. 

Based on which state variables are actually present in the 

observation, a unique binary signal is generated. Take the 

coefficient of the power 

i _ 1 _ , 1 if x i is present in the observation 
2 “ 0 if x x is not present 

The number is stored with the position vector (x) as low bit. 
For example: 



SRC = 11 
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The binary number implies that x, x and y make up the 
observation. This allows subsequent decoding of which 
components are present by a combination of divide by 2 and 
modulo opus. 

The matrix H, can be produced for any combination of 
measurements. Given the same statistical characteristics as 
the actual data, it is possible to have missing data and 
multiple returns from adjacent arrays. This filter has the 
ability to continue when pieces of data are not received, or 
when the two sensors are operating at slightly different 
frequencies. The scheme used here helps in this regard in 
that it is more robust. 
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V. RESULTS 



A. BASELINE INFORMATION 

The s-curve data is run through the filter from [Ref 1, 
pp. 61]. This information will now be used in determining 
improvements in performance. The knowledge that the filter 
performed best with the excitation matrix weighted to 1500, 




maneuver detection scheme 

performance of the filter can be improved in this noiseless 
environment by increasing the figure for the weighing from 
1500. However, as shown in [Ref 1], the filter's performance 
in a noisy environment would be degraded. 



21 



For a better idea of what is causing the errors, Figure 
5.2 shows when the target is maneuvering. Looking at where 
the maneuvers occur, coincides with the disturbance in 
tracking. 



„ X Acceleration vs. Time 

O' 20 T i 1 1 r i r 

CN 

< 




_2Q I 1 1 * 1 1 1 L 



0 5 10 15 20 25 30 35 40 45 

Time (sec) 




Time (sec) 

Figure 5.2: Target's acceleration 

B. MODIFICATIONS 

The filter's maneuver detection scheme and observation 
measurement matrix, (H) , are now as discussed in the previous 
chapter. This filter is modified in one other respect. This 
filter manipulates the excitation matrix when a maneuver is 
detected, not after the maneuver is detected. The purpose of 
estimating the acceleration is to be able to sense the 



22 



maneuver simultaneously with the onset of the maneuver. "None 
of the time lag associated with the residual method is 
experienced." [Ref 1] The results are readily seen in Figure 
5.3. Again, this is with no noise and the Q matrix is 
weighted at unity. A view of the targets path is provided in 
Figure 5.4. 
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Figure 5.3: X and Y error for the s-curve using the new 

filter; weight is set at unity 



Note that the smoothing in backward filter causes the 
larger of the errors in a noiseless environment. The more the 
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excitation matrix is weighted the less the errors become. 
But, again, this does not hold true with noise in the 
environment and the trade offs need to be evaluated. 

C. EVALUATION FOR OPTIMAL PERFORMANCE 

In order to evaluate the differences in the two filters, 
we must first establish the optimal performance for this new 
filter. Multiple tracks are created to establish a value for 
the weight and to gather enough data to reduce any statistical 
errors . 

As discussed in [Ref 1, pp. 31] Gaussian noise is added to 
the track with a distribution variances of 15 square feet, 
0.01 square yards/second, and 0.03 square feet/second squared 
for position, velocity, and acceleration respectively, with 
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zero mean. 



A figure of such a track is provided in Figure 



5.5. 



x (-) ond xrondom ( — ) 




x-oxis 

Figure 5.5: Target track with noise added 



Trial and error is used to find the optimal performance of 
the filter. The numbers seen in Table 5.1 are the mean radial 
distance errors obtained from the program in [Ref 1, pp. 100]. 
Table 5.1 shows a few of the multiple iterations of 
combinations used in choosing the optimal weights. The 
maximum weight (WTMAX) and minimum weight (WTMIN) are chosen 
to be 100 and 0.1 respectively. This choice offers a half 
foot reduction in mean radial distance error in a relatively 
quiet environment, while giving up a little less than a half 
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foot in this noise filled environment. Figure 5.6 shows how 
the filter handles a noisy track for one run. 



TABLE 5.1: Table of weights 







Maximum Weight 






500 


100 


10 


1 


0.1 


0.01 




100 


4.27 


4.03 












10 


3 . 18 


2.86 


2 . 3 


-- 


— 




M 


1 


3 


2 . 67 


2 . 3 


2 . 23 


— 


— 


I 

N 


0.1 


2.96 


2.66 


2.28 


2 . 2C 


2.22 


— 


. 01 


2.98 


2.67 


2.28 


2.22 


2.31 


3 . 36 
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Figure 5.6: Error for noisy track 



D. COMPARISONS 

Both filters, using all of their best features, are used 
on a track with approximately 20 feet of mean radial distance 
error. Figure 5.7 provides a visual comparison as to the 
capability of each filter in the same figure. The mean radial 
distance errors in the upper left hand corner show the 
difference in performances of the old and new filters. 
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Figure 5.7: A comparison between mean radial distance of the 
old and new filter 



For the circular maneuver presented in [Ref l,pp. 36], we 
will look at the best result for the old filter with optimal 
settings. In this track the torpedo makes 360 and 270 plus 
degree turns. The old mean radial distance error was 5.951 . 
In Figure 5.8, the mean radial distance errors are in the 
upper left hand corner. The old track is the "raw" 
observation while the new track is the smoothed filter 
estimate of the true track. The mean radial distance error 
show over a 50% improvement with an error of 2.165 . 
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X-Axis 

Figure 5.8: Filter output with WTMIN=0.1 and WTMAX=100.0 
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VI. CONCLUSIONS AND RECOMMENDATIONS 

The objective was to investigate uses of fusion in 
tracking problems. In the process, there were several items 
that bore out special investigation. 

The program for torpedo track reconstruction now works in 
three dimensions and its overall performance improved. One 
major improvement was the creation of the OBSDATA. FOR program 
and its subsequent improvement on the observation or 
measurement matrix. This revamping of the program and its 
logic has improved its overall characteristic another 50 to 
80%. 

The tracking of targets by electromagnetic and infra-red 
sensors needs to be pursued. The sensors in [Ref 4], all have 
the capability of tracking fast moving targets. The 
preliminary results look good for three polar coordinate 
filters, all working in parallel, to track air traffic 
passively. If there were one filter per sensor, and one 
filter to fuse and triangulate bearings using a conversion 
vector like in [Ref 4, pp.4-4], then vessels would be able to 
track traffic passively in conditions of restricted 
electromagnetic emissions. 
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APPENDIX A: DEFINITION OF TERMS FOR USE IN TORPMAN . FOR 



The terms and characters defined here are for easy access 
to those trying to comprehend the program in Appendix C. The 
logic for the program is discussed in more depth in Chapter 
IV. 

TABLE A.l: Terms used in TORPMAN. FOR 



$3DTEMP 


This is a direct access file for storing 
values of the error covariance matrix (P k \ k ) . 
The character associated with this file is 
PKKS3D. 


$M1TEMP 


This is a direct access file for storing 
values of PKKM1S or the smoothed P |cXk . 1 . 


ADD 


A subroutine which adds two input matrices. 



ACCTHRSH The acceleration threshold is initialized to 





5 feet/second. This is for use in the 
maneuver detection program, MANDET, and may 
be changed within the SETTHRSH subroutine. 


CHANGE 


This is a subroutine which allows the user b 
change the weight (WTMIN or WTMAX) and error 
covariance matrix at time k minis 1 (PKKM1) . 
The program is therefore altered in behavior 
without having to compile. 


H 


The measurement matrix is discussed in more 
depth in chapter V. 


NDIMEN 


The number of dimensions used are 1, 2, or 3 
D. 


NUMSTA 


This is the number of states in the vector 
space. 


NSTMAT 


This is used in FORTRAN memory allocation fir 
one time step. 
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NOBVEC 



OBSERVAT.DAT 
PHI DEL 
PKK 
PKKM1 

PKKM1S 

PKKOUT.DAT 

Q 

R 

ROLTHRSH 

SCR 

TMPV1 & 2 
TEMPI, 2, & 3 
WTMAX 

WTMIN 

XKK 



This allocates an array large enough for the 
observed data set. 

The observed data base. 

This subroutine builds the Phi and Del natrix. 

The error covariance matrix (P kvk ) . 

The error covariance matrix (P kNk _.,) , which is 
at time minus one. 

This is the smoothed PKKM1 from the backward 
Kalman filter. 

An output file is created containing the 
information for error ellipse in the x and y 
plane. 

State excitation noise matrix. 

Observation noise matrix. 

The roll threshold is initialized to 5 
degrees/second. This can be changed with the 
SETTHRSH subroutine. 

An integer, when read as a binary number, 
reveals the make up of the observation. 

A temporary storage space for characters as 
they change value from k/k-1 to k/k to k+l/k 
= k/k-1 . 

A temporary storage space for characters as 
they change value from k/k-1 to k/k to k+l/k 
= k/k-1 . 

This is the maximum weight used upon the 
detection of a maneuver. The value is preset 
to 20,000 but can be altered in the CHANGE 
subroutine. 

This is the minimum weight used in the 
program. Which insures that the Kalman gains 
do not approach zero if the target is not 
maneuvering. 

The state estimate (£ kNk ). 
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XKKM 1 


The state estimate (-#1^-1) , which is at 
time minis one. 


XKKFWD . DAT 


The state estimates of the Kalman filter are 
stored in this data file. 


XKKS 


Observations are stored in this matrix, 
including acceleration obtained from AV. It 
is then used for the state estimate and 
smoothed state estimate as it passes through 
forward and backward Kalman filter. 


XKSMOOTH . DAT 


The state estimates of backward filtering 
(XKKS) are stored in this file. 


ZZ 


Stores the observations. 
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APPENDIX B: MAT LAB PROGRAMS 



A. ANALYZE. M 

%analyze.m 

% This M-file compares the output of a Kalman Filter Run 

% with the true x, y, and z positions. 

clg 

Idel data\six.met 
! cd data 
load tracknum.dat 
tracknum = fix(tracknum) ; 
load trueposn. 
load xkkfwd.dat 
load xksmooth.dat 
% 

errfwd=xkkfwd( 2 :4) -trueposn ; 
errbwd=xksmooth ( : , 2 : 4 ) -trueposn ; 

% 

eval ( [ '! del filtr ' , num2str (tracknum) .met ' ]) 

subplot (211) ; 

plot (xkkfwd ( : , 1) , errfwd ( : , 1) , ' — g' , . . 

xkkfwd(:,l), errbwd( : , 1) , '-.w' ) 
xlabel('Time (sec) 1 ) ;ylabel ( ' X-Error Magnitude' ); 
title (' Foirward ( — ) and Backward (-.) X Error vs. Time')? 

% 

plot (xkkfwd ( : , 1) , errfwd ( : , 2) , ' — g ' , . . 

xkkfwd ( : , 1 ) , errbwd (:,2),'-.w') 
xlabel('Time (sec) ' ) ;ylabel ( ' Y-Error Magnitude' ); 
title (' Forward ( — ) and Backward (-.) Y Error vs. Time'); 
eval ( [ 'meta filtr ', num2str (tracknum) ]) 
meta six 
pause 
% 

clg 

subplot (211) 

plot (xkkfwd ( : , 1) , errfwd ( ; , 3) , ' — g ' , . . 

xkkfwd ( : , 1 ) , errbwd (:,3),'-.w') 
xlabel('Time (sec) ' ) ;ylabel ( ' Z-Error Magnitude' ); 
title (' Forward ( — ) and Backward (-.) Z Error vs. Time'); 
%meta 
pause 
clg 
% 

eval ( ' I cd . . ' ) 
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B. ELLIPSE. M 

This program will plot an error ellipsoid once ever six 
observations. 

!del pic2.m§t 

load c : \matlab\wiseman\output2 . dat 
load c : \matlab\wiseman\truepos2 . dat 
load c:\matlab\wiseman\jokkout.dat 
axis( 'square' ) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% plot path 'and estimate of path % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
plot (output2 ( : , 1) , output2 ( : ,2) , •+ 1 , 

truepos2 ( : , 2 ) , truepos2 ( : , 3 ) ) 
xlabel(’X - Position ( ft) ' ) ;ylabel ( ' Y - Position (ft)') 
title ( 'Trajectory - S Curve') 

gtext(' True track') 

gtext('+ + + Estimate') 
gtext('0 Error ellipsoid') 
gtext('20 times larger') 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% generate a uniformly spaced set of points % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
delt=-l : .05:1; 
delt=pi*delt ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% generate a (2xm) set of vectors on the unit circle % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

y=[cos(delt) ;sin(delt) ] ; 

shift=diag (ones (41) ) ; 

shift=shift ' ; 

hold on 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 



% The function chol, to compute the Choleski % 
% decomposition, is done "backwards” in MATLAB. It % 
% makes an upper triangular matrix R, not L. So take % 
% the transpose. % 
% An array of x-vectors (2xm) has been created that % 
% form the error ellipsoid. % 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
for k=6:6:max(size(output2) ) , 

P=pkkout(k*2-l:k*2,l:2) ; 
s=chol (P) ' ; 
x=s*y ; 

plot (x(l, : ) *20+output2 (k, 1) *shift, 

x ( 2 , : ) *20+output2 (k,2) *shift) 

end 

hold off 
axis ( 'normal ' ) 
meta pic2 
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c 



GENFILES.M 



% THIS PROGRAM CONVERTS SIMULATED TORPEDO TRACKS GENERATED BY 
% THE MATLAB PROGRAM TORPGEN . M INTO FILES SUITABLE FOR INPUT 
% TO THE FORTRAN PREPROCESSOR OBS DATA . FOR . 

% AFTER CONVERSION INTO STANDARD INPUT FORMAT BY OBS DATA . FOR , 
% THE DATA WILL BE TESTED AGAINST THE KALMAN FILTERING PROGRAM 
% TORPMAN . FOR , WHICH IS DESIGNED TO RECONSTRUCT TORPEDO TRACKS 
% GENERATED AT THE UNDERSEA RANGE AT KEYPORT, WA. 

% 

% 

% 

%%%%%%% Prompt User for Desired Type and Run %%%%%%%%%%%%%% 
%%%%%%% Number for Data File %%%%%%%%%%%%%% 

% 

! els 

disp(' Select Type of Simulated Track to Process ' ) 
disp( 1 1 ) 

disp(' 1. True Track (w/o pseudo-random noise added)' ) 

disp(' 2. Track with Pseudo-Random Noise Added' ) 

disp(' (Any other value will abort processing)') 
disp ( ' ' ) 

ntrktyp = input (' Selection: ') ; 

if ntrktyp <= 0 ; return ; end 
if ntrktyp > 2 ; return ; end 
% 

disp ( ' •); 

trutrk = input ([' Enter the Sequence Number of the True Track 

File: ']) 

obsfil = [ 'true' ] ; 
obstrk = trutrk; 
if ntrktyp == 2 
disp ( ' ' ) ; 

obsfil = [ ' tst ' ] ; 

obstrk = input ([' Enter the Sequence Number of the Simulated 

Track File: ']) ; 

end 



% 

%%%%%%%%%% Input the Desired Files and Produce 
%%%%%%%%%% Standard Output Files That Will Run 
%%%%%%%%%% with Program POSCONV.FOR 

% trueposf = [ 'truep' , int2str (trutrk) ] ; 

deltatf = [ 'delt ', int2str (trutrk) ] ; 
eval ( [ 'load data\' , trueposf ]) 
eval( [ 'load data\' , deltatf ]) 

% 

save data\trueposn x /ascii 

save data\deltsimu deltat /ascii 

% 

positionf = [ obsfil ,' p' , int2str (obstrk) 
velocityf = [ obsfil , 'v' , int2str (obstrk) 



%%%%%%%%%% 

%%%%%%%%%% 

%%%%%%%%%% 



] ; 
] ; 
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% 



% 

A Z 



acceleraf = [ obsf il , ' a ' , int2str (obstrk) ] 
eval( [ 'load data\' ,positionf ]) 
eval ( [ 'load da ta\ ', velocity f ]) 
eval( [ 'load data\ ', acceleraf ]) 



save 

save 

save 

save 



data\tracknum . dat 
data\simulpos x 
data\simulvel v 
data\simulacc a 



obstrk /ascii 
/ascii 
/ascii 
/ascii 
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D. MISSILE1 . M 



MISSILE1 . M 

This program was developed for tracking a target moving at 
approximately mock three. The problem is set up for the poler 
coordinate system. 

dtl=0 . 01 ; 
kmax=100 ; 

%Missile 

speed=l; %approx. 3.3 time the speed of sound 

xrange=0; %km 

yrange=20; %km 

x=zeros ( 4 , kmax) ; 

x( : , l)=[xrange 3*speed/5 yrange -4*speed/5] ' ; 
theta=zeros(l,kmax) ; 
theta(l, l)=atan2 (x(l,l),x(3,l)); 
time=zeros(l,kmax) ; 

A= [ 0 1 0 0 ; 0 0 0 0 ; 0 0 0 1 ; 0 0 0 0 ] ; 

B= [ 0 0 ; 1 0 ; 0 0 ; 0 1 ] ; 

[phi,del]=c2d(A,B,dtl) ; 

%Filter 

R=0.001; 

Q=0 ; 

A= [ 0 1 0 ; 0 0 1 ; 0 0 o ] ? 

B= [0 0 1 ] 1 ; - 

H=[ 1 0 0 ] ; 

[Phi, Del]=c2d(A, B,dtl) ; 

G=zeros ( 3 , kmax) ; 

XKK=zeros ( 3 , kmax) ; 

XKKMl=zeros ( 3 , kmax) ; 

PKKMl=eye ( 3 ) * le7 ; 
for k=l : kmax 

G ( : , k) =PKKM1*H ' * (H*PKKM1*H'+R) A (-1) ; 

PKK= ( eye ( 3 ) -G ( : ,k) *H) *PKKM1; 

PKKMl=Phi*PKK*Phi ' +Q; 

XKK ( : , k) =XKKM1 ( : ,k)+G( : ,k) * (theta (k) -XKKMl(l,k) ) ; 

XKKM1 ( : , k+1) =Phi*XKK( : ,k) ; 
x( : ,k+l)=phi*x( : ,k) ; 

theta (k+1) =atan2 (x(l,k+l) ,x(3,k+l) ) ; 
time (k+1) =time(k)+dtl; 

end 

plot(x(l, :) ,x(3, :)) , grid, y label ( 1 ( Y in (km))') 
title (' Targets track' ), xlabel (' (X in (km))') 
meta testl 

plot (time, theta*180/pi) , grid, title (' Bearing to thetarget') 
xlabel ( ' (Sec) ' ) , ylabel ( ' Degrees ' ) 



38 



meta test2 

plot (time (1,1: kmax) , XKK ( 1 , : ) *180/pi) , grid 
title ( 1 Kalman filter estimate of bearing') 
xlabel ( • (Sec) ' ) , ylabel ( ' Degrees ' ) 
meta test3 
look=10; 

plot (time (1, 1: look) ,G(1, l:look) ) , title ( 'G(l, : ) ' ) 
xlabel ( ' (Sec) ' ) 
meta test6 

plot (time (1,1: look) ,G(2, l:look) ) , title ( 'G(2, : ) • ) 
xlabel ( ' (Sec) ' ) 
meta test7 

plot (time (1, 1: look) , G (3 , 1: look) ) , title ( ' G ( 3 , : ) ' ) 
xlabel ( ' (Sec) ' ) 
meta tests 

plot (XKK ( 2, :)) , title ( 'Estimate of Angular rate') 
xlabel (' (Samples taken)') 
meta test4 

plot(XKK ( 3, :)) ,title( 'Estimate of angular acceleration') 
xlabel (' (Samples taken)') 
meta test5 
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E. MISSILE2.M 



This program was developed for tracking a target using fusion. 
The two sensors are located at the origin. 

dtl=0 . 01 ; 
dt2=l ; 
kmax=3000 ; 

%Missile 

speed=l; %approx. 3.3 time the speed of sound 

xrange=0 ; %km 

yrange=20; %km 

x( : , 1) =[xrange 3*speed/5 yrange -4*speed/5 ] ' ; 
theta (1, : ) =atan2 (x (1 , 1) ,x(3,l) ) ; 

A= [ 0 1 0 0 ; 0 0 0 0 ; 0 0 0 1 ; 0 0 0 0 ] ; 

B= [ 0 0 ; 1 0 ; 0 0 ; 0 1]; 

[phi , del ] =c2d (A, B, dtl) ; 

%Filter 
i=l ; 

R= .01; 

Q=0 ; 

A= [ 0 1 0 ; 0 0 1 ; 0 0 0 ] ; 

B= [ 0 0 1 ] ' ; 

H= [ 1 0 0 ] ; 

[ Phi , Del ] =c2d (A, B, dtl) ; 

XKKMl=zeros (3,1) ; 

PKKMl=eye ( 3 ) *le7 ; 

Phi_IR=eye ( 3 ) ; 
count=0 ; 
for k=l:kmax 

G ( : , k) =PKKM1*H ' * (H*PKKM1*H ' +R) A ( -1) ; 

PKK= ( eye ( 3 ) -G ( : ,k) *H) *PKKM1; 

PKKMl=Phi*PKK*Phi ' +Q ; 

XKK ( : ,k)=XKKMl ( : ,k)+G( : , k) * (theta (k) -XKKM1 ( 1 , k) ) ; 
if count*100+l == k 

G ( : , i)=PKKMl*H'*(H*PKKMl*H'+R) A (-1) ; 

PKK=(eye(3) -G( : , i) *H) *PKKM1; 

PKKMl=Phi_IR*PKK*Phi_IR ' +Q ; 

XKKM1 ( : , i)=XKK( : , k) ; 

XKK ( : ,i)=XKKMl(: ,i)+G(: , i) * (theta (k) -XKKM1 ( 1 , i) ) ; 

XKKM1 ( : , k+1) =Phi*XKK( : , i) ; 
count=count+l ; 
i=i+l ; 
else 

XKKM1 ( : , k+1) =Phi*XKK( : ,k) ; 

end 

x( : ,k+l) =phi*x( :,k); 

theta (k+1) =atan2 (x(l,k+l) ,x(3,k+l) ) ; 
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end 

subplot (221) 

plot(x(l f :) ,x(3, :) ) , grid, ylabel ( * ( Y in (km))') 

title ( 'Missiles track '), xlabel (' (X in (km))') 

plot (theta*180/pi) , grid, title (' Bearing to the missile') 

xlabel (' Samples taken (t=. 01) ') , ylabel (' Degrees ' ) 

plot (XKK(1, : ) *180/pi) ,grid 

title ( 'Estimate (For Radar)') 

xlabel (' Samples taken (t=. 01) ') , ylabel (' Degrees ' ) 
for k=l: 500 
t (k) =dtl*k; 

end 

plot (t,G (1 , 1:500)) , title ( 'G(l, : ) ' ) , pause 
plot (t,G (2, 1:500) ) , title ( 'G(2, :) ' ) 
plot (t,G( 3, 1:500) ) , title ('G(3, :) ') 
plot(XKK(2, : ) ) ,title( 'Angular velocity' ) 
plot (XKK ( 3 , : ) ) ,title( 'Angular acceleration' ) 
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F. S_ELLIPSE.M 

This program will produce error ellipsoids for the first 
few samples of the track. 

echo off 
!del pic3.met 

load c : \matlab\wiseman\output2 . dat 
load c : \matlab\wiseman\truepos2 . dat 
load c : \matlab\wiseman\pkkout . dat 
r=9 ; 

axis ( ' square ' ) 
axis ( [ -50 500 -50 500]) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% plot path and estimate of path % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
plot (output2 (l:r, 1) ,output2 (l:r,2) , , 

truepos2 ( 1 : r , 2 ) , truepos2 ( 1 : r , 3 ) ) 
xlabel('X - Position (ft) ' ) ;ylabel ( ' Y - Position (ft)') 
title ( 'Trajectory - S Curve') 

gtext(' True track') 

gtext('+ + + Estimate') 
gtext('0 O Error Ellipsoids') 
gtext (' Enlarged 10 times') 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% generate a uniformly spaced set of points % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
delt=-l : . 05 : 1 ; 
delt=pi*delt ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% generate a (2xm) set of vectors on the unit circle % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
y=[cos(delt) ;sin(delt) ] ; 

shift=diag (ones (41) ) ; % 41 points in delt 

shift=shift ' ; 
hold on 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% The function chol, to compute the Choleski % 

% decomposition, is done "backwards" in MATLAB. It % 

% makes an upper triangular matrix R, not L. So take % 

% the transpose. % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
for k=l:9 
i=k-l; 

P=pkkout (2*i+l : 2*i+2 ,1:2) ; 
s=chol (P) ' ; 
x=s*y ; 

plot(x(l, : ) *10+output2 (k, 1) *shift, 

x(2 , : ) *10+output2 (k, 2) *shift) 

end 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% An array of x-vectors (2xm) has been created that % 

% form the error ellipsoid. % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

hold off 

pause 

meta pic3 
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G. TORPSCUR.M 



This is the program to generate a true track and a monte 
carlo track. The program generates a simulation for a 40 knot 
torpedo following an s-shaped track. 

Velocity is in yards/second and position is in feet. Data 
orientation is similar to actual data, 
ndim = 3; 



% 

% 

%%%%% Establish Measurement Standard Deviations %%%%%%%%% 
% 

sigmapos = diag([ 15.0 , 15.0 , 0.00 ]) ; 

sigmavel = diag([ 0.01 , 0.01 , 0.00 ]) ; 

sigmaacc = diag([ 0.03 , 0.03 , 0.00 ]) ; 

% 

%% Let User Input Number of "Noisy" Tracks to Generate %% 

%% and the Seed for the Random Number Generator %% 

% 

nsimul = input ([' Enter the Base Reference Number for 

this Simulation (100 <= n <= 900): ']) ; 

nsimul = fix (min (max (nsimul, 100) , 900) ) ; 

fprintf ( 1 Your Base Reference Number is - %4.0f\n', 
nsimul) ; 

% 

ntrack = input ('Enter Number of Random Tracks to 

Generate: ') ; 

nseed = 0 ; 

if ntrack > 0 ; * 

nseed = input ('Enter Seed for Random Number 

Generator: '); 

end 



% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 



%%%% The Following Section Generates the "True" %%%%% 
%%%% and Simulated Tracks. %%%%% 
%%%% This Section Must Be Changed as Appropriate %%%%% 
%%%% in Order to Generate Other Tracks %%%%% 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%%%%%%%% 

%%%%%%%%%%%%% Establish Model Constants %%%%%%%%%%%%%%% 

% 



r 

deltat 

tf inal 

kmax 

ydconv 

v40kt 

thetadot 



250; 

0.5; 

45.0; 

tfinal/deltat + 1; 



3.0; 



200/9 ; 

ydconv*v40kt/r ; 
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% 

%%%%%%%%%%%%% Initialize Data Arrays %%%%%%%%%%%%%%% 

% 

x = zeros (kmax, ndim) ; 

v = zeros (kmax, ndim) ; 

a = zeros (kmax, ndim) ; 

% 

%%%%% Generate Accelerations for a S-Track 

% 

v(l, 1:2) = [ 30 30 ] ; 

theta = thetadot*deltat* ( (0:119 )'+0.5) ; 

a(25:34,l) = -12 . *ones(10, 1) ; 

a ( 45 : 54 , 2 ) = -12 . *ones (10, 1) ? 

a(65:74, 1:2) = ones(10,l)*[ 12. 12. ]; 

% 

% 

%%%%% Then Generate Velocities and Positions by 
%%%%% Piecewise Constant Integration 

% 

% 

for i=2:kmax ; 

v(i,:) = v(i-l,:) + a (i-1, : ) *deltat/ydconv ; 

x(i,:) = x(i-l,:) + ydconv*v ( i-1 , : ) *deltat ... ; 

+ a (i-1, : ) *deltat A 2/2 ; 

end 

% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%%%%% Plot the "True" Track %%%%% 

% 

eval( [ '! del data\track' , num2str (nsimul) , ' .met ' ]) 
plot (x (1 :kmax, 1) , x ( 1 : kmax, 2 ) ) ;xlabel ( ' x-axis ' ) ; 
ylabel ( 'y-axis ' ) ;grid; 

eval( [ 'meta data\track* ,num2str (nsimul) ]) 

% 

% and Save the True Track Position/Velocity/Acceleration %% 
% 

eval( [ 'save data\delt ', num2str (nsimul) , ' deltat ' ]) 
eval( [ 'save data\truep ', num2str (nsimul) , ' x ' ]) 

eval ( [ 'save data\truev' ,num2str (nsimul) , ' v ' ]) 

eval ( ( 'save data\truea ', num2str (nsimul) , ' a ' ]) 

% 

%%%%% Next Generate the Requested Number of "Noisy" %%%%% 



%%%%% Tracks by Adding Random Noise with the %%%%% 
%%%%% Previously Specified Standard Deviations to %%%%% 
%%%%% the "True" Track. (This is done is a loop, %%%%% 
%%%%% with each track graphed and saved as soon as %%%%% 
%%%%% it is generated.) %%%%% 
% 

truex = x ; truev = v ; truea = a ; 
rand ( • normal ' ) ; 



%%%% 



%%%%% 

%%%%% 
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% 



% 

% 



% 



if ntrack > 0 ; 

rand ( ' seed ' , nseed) ; 

[ 'save data\seed' , num2str (nsimul) , ' nseed ' ]) 
j=l: ntrack ; % Generate Tracks 

truex + rand(kmax, 3) *sigmapos ; 
truev + rand(kmax, 3) *sigmavel ; 
truea + rand(kmax, 3) *sigmaacc ; 

% Then Save them, using 
% the 'eval' function 
data\tstp ' , num2str (nsimul+j ) , ' x ' ]) 
data\tstv' , num2str (nsimul+j ) , ' v ' ]) 
data\tsta' ,num2str (nsimul+j) , ' a 1 ]) 



eval ( 
for 
x = 

V = 

a = 



eval ( 
eval ( 
eval ( 



' save 
' save 
' save 



clg ; % Then Plot Them 

eval ( [ '! del data\track' ,num2str (nsimul+j) .met' ]) 



plot ( truex ( 1 : kmax , 1 ) , truex ( 1 : kmax , 2 ) , x ( 1 : kmax , 1 ) , x ( 1 : kmax , 2 ) 
— '), title ('x (-) and xrandom ( — )') ; 

xlabel ( ' x-axis ' ) , ylabel ( ' y-axis ' ) , grid 

eval ( [ 'meta data\track' ,num2str (nsimul+j ) ]) 
end ; end 

% 
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H. TRACKS IM.M 

% 

% TRACKS IM.M 

% THIS PROGRAM GENERATES SIMULATED TORPEDO TRACKS FOR 

% TESTING THE KEYPORT KALMAN FILTERING. THIS PROGRAM AS 
% CURRENTLY WRITTEN GENERATES A SIMULATION FOR A 40 KNOT 
% TORPEDO ENTERING A 250 FT RADIUS CIRCLE AT A TURNING RATE 
% OF 15 DEGREES/SECOND. THE VELOCITY IS IN YARDS/SECOND 
% AND POSITION IS IN FEET, AND THE DATA ORIENTATION IS 
% SIMILAR TO ACTUAL DATA. HOWEVER, THE PROGRAM IS DESIGNED 

% SO THAT ONE SIMPLY NEED CHANGE THE ACCELERATION DATA (IN 

% THE PORTION OF THE CODE WHERE INDICATED) 

% 

% 

ndim = 3 ; 

% 

% 

%%%%%% Establish Measurement Standard Deviations %%%%%%%%% 

% 

sigmapos = diag([ 15.0 , 15.0 , 0.00 ]) ; 

sigmavel = diag([ 0.01 , 0.01 , 0.00 ]) ; 

sigmaacc = diag([ 0.03 , 0.03 , 0.00 ]) ; 

% 

%%% Let User Input Number of "Noisy" Tracks to Generate %%% 
%%% and the Seed for the Random Number Generator %%% 

% 

nsimul = input ([ 'Enter the Base Reference Number for 

this Simulation (100 <= n <= 900): ']) ; 

nsimul = fix(min(max(nsimul, 100) ,900) ) ; 

fprintf ( ' Your Base Reference Number is - %4 . 0f\n' , nsimul) ; 

% 

ntrack = input ('Enter Number of Random Tracks to 

Generate : ' ) ; 

nseed = 0 ; 

if ntrack > 0 ; 

nseed = input ('Enter Seed for Random Number 

Generator: '); 

end 

% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 



%%% The Following Section Generates the "True" %%% 
%%% and Simulated Tracks. %%% 
%%% This Section Must Be Changed as Appropriate %%% 
%%% in Order to Generate Other Tracks %%% 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%%%%%%%%%%%%%% Establish Model Constants %%%%%%%%%%%%%%% 
% 

r = 250; 
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deltat = 0.5; 

tfinal = 80.0; 

kmax = tf inal/deltat + 1; 

ydconv = 3.0; 

v40kt = 200/9 ; 

thetadot = ydconv*v40kt/r ; 

%%%%%%%%%%%%%% Initialize Data Arrays %%%%%%%%%%%%%% 



X 


= zeros ( kmax ,ndim) ; 




V 


= zeros (kmax, ndim) ; 




a 

% 


= zeros (kmax, ndim) ; 




% 

%%%% 


Generate Accelerations for a Circular Track 


%%%%% 



% 



v(l,:) = [ v40kt ,0,0]; 

theta = thetadot*deltat* ( (0:119 )'+0.5) ; 

a (20, 1:2) = [ -v40kt/deltat , v40kt/deltat 

* ydconv ; 

a ( 21 : 140 , 1 : 2 ) = ( (v40kt*ydconv) A 2/r ) 

*[ -cos (theta) , -sin (theta) 

% 

% 

%%%%% Then Generate Velocities and Positions by 
%%%%% Piecewise Constant Integration 

% 

% 



] 

] 7 

%%%%%%%%% 

%%%%%%%%% 



for i=2 : kmax 
v ( i , : ) 
x ( i , : ) 



end 



v(i-l,:) + a(i-l, : ) *deltat/ydconv 
x(i-l,:) + ydconv*v (i-1 , : ) *deltat 
+ a ( i-1 , : ) *deltat A 2/2 ; 



% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%%%%%% Plot the "True” Track %%%%%%%%%%% 

% 



eval( [ '1 del da ta\track' ,num2str (nsimul) ,' .met ' ]) 

plot (x(l : kmax, 1) ,x( 1 :kmax, 2) ) ;xlabel ( 'x-axis ' ) ; 
ylabel ( 'y-axis ' ) ;grid; 

eval ( [ 'meta data\track' ,num2str (nsimul) ]) 

% 

% and Save the True Track Position/Velocity/Acceleration %% 



eval( [ 'save data\delt ', num2str (nsimul) , ' deltat ' ]) 
eval( [ 'save data\truep ', num2str (nsimul) , ' x ' ]) 

eval ( [ 'save data\truev' , num2str( nsimul) , ' v ' ]) 

eval( [ 'save data\truea ', num2str (nsimul) , ' a ' ]) 

% 

%%%%% Next Generate the Requested Number of "Noisy" %%%%% 

%%%%% Tracks by Adding Random Noise with the %%%%% 
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%%%%% Previously Specified Standard Deviations to %%%%% 
%%%%% the "True" Track. (This is done is a loop, %%%%% 

%%%%% with each track graphed and saved as soon as %%%%% 

%%%%% it is generated.) %%%%% 

% 

truex = x ; truev = v ; truea = a ; 
rand( 'normal' ) ; 

% 

if ntrack > 0 ; 

rand ( ' seed ' , nseed) ; 

eval( [ 'save data\seed ' ,num2str (nsimul) , ' nseed ' ]) 
for j=l: ntrack ; % Generate Tracks 

x = truex + rand(kmax, 3) *sigmapos ; 
v = truev + rand (kmax, 3) *sigmavel ; 
a = truea + rand (kmax, 3) *sigmaacc ; 

% % Then Save 

them, using 

% % the 1 eval ' 

function 

eval ( [ 'save data\tstp' ,num2str (nsimul+j ) , ' x ' ]) 

eval( [ 'save data\tstv' ,num2str (nsimul+j ), ' v ' ]) 

eval ( [ 'save data\tsta' ,num2str (nsimul+j ) , ' a ' ]) 

% 

clg ; % Then Plot Them 

% 

eval ( [ '! del data\track' ,num2str( nsimul+j) ,' .met' ]) 

plot (truex (l:kmax, 1) , truex ( 1 : kmax , 2 ) , x (1: kmax, 1) , x (1 : kmax, 2 ) 

; 

title('x (-) and xrandom ( — )') ; 

xlabel ( ' x-axis ' ) , ylabel ( 'y-axis ' ) , grid 

eval ( [ 'meta data\track' , num2str (nsimul+j ) ]) 

end ; end 

% 
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APPENDIX C: THE KALMAN FILTER AND MANEUVER DETECTION 



A. OBSDATA. FOR 

C OBSDATA. FOR 

C MODIFICATIONS MADE BY ART SCHOENSTADT ON AUGUST 14, 1992. 
C 

C THIS PROGRAM IS DESIGNED TO PROCESS 3/D DATA FILES WHICH 

C SIMULATE THE UNDERSEA TRACKING RANGE AT KEYPORT, WA. 

C THE INPUT DATA FILES ARE GENERATED BY A MATLAB SIMULATION. 

C THIS PROGRAM IS DESIGNED TO RUN ON THE IBM/AT PERSONAL 

C COMPUTER. THE USER IS ALSO REQUIRED TO PROVIDE THE INPUT 

C DATA FILES IN SPECIFIC FORMAT, AND THE NUMBER OF POINTS TO 

C CALCULATE (MAXIMUM OF 2500) . THE PROGRAM USES A 

C LARGE AMOUNT OF HARD DISK SPACE FOR TEMPORARY 

C FILES (APPROXIMATELY 160K BYTES/100 POINTS, 4 MEG BYTES MAX 

C FOR 2500 POINTS. 

C THE PROGRAM ALSO INCLUDES "HARD WIRED" CONVERSIONS TO 

C REFLECT THE FACT THAT SOME DATA IS IN FEET, OTHER DATA IN 

C YARDS, AND DEPTH IS MEASURED IN POSITIVE, NOT NEGATIVE 

C TERMS . 

C 

C**************** SECTION 1 ************************** 



C 

C*** DECLARATION OF PARAMETERS *** * 

C*** MAXOBS IS THE MAXIMUM NUMBER OF OBSERVATIONS THAT *** 
C*** CAN BE USED *** 

C*** MAXSTATE IS SET FOR A 3-DIMENSIONAL, POSITION, *** 

C*** VELOCITY AND ACCELERATION PLANT *** 

C*** DEPS IS USED TO TEST WHEN TWO OBSERVATIONS ARE CLOSE *** 
C*** ENOUGH TO BE CONSIDERED SIMULTANEOUS *** 



C 

PARAMETER ( MAXOBS=250, MAXSTATE=9 ) 

PARAMETER ( DEPS=0 . 005D0 , STEP=0.5D0 ) 

C 

C ***** DECLARATION OF VARIABLES ***** 

C 

DOUBLE PRECISION PTC (MAXOBS) , VTIME (MAXOBS ) , 

* SOURCE (MAXOBS) , ZK (MAXSTATE, MAXOBS) , ROLL(MAXOBS ) 
DOUBLE PRECISION SS , SSV, SEC , X , Y , Z , XVEL, 

* YVEL, ZVEL, ACCX, ACCY , ACCZ , EYAW, EROLL, EPITCH , PTM, VTM, 

* ATM , RTM , ATM1 , ACCXM1 , ACC YM1 , ACCZM1 , RLLM1 , SSA , SSR , 

* TORANGE , TOTORP , TNEXTOBS 
C 

INTEGER HH , HHV , MM , MMV , HOUR , MIN , PC , MMA , MMR , NOBSERV , SRC 
C 
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CHARACTER ASK*1 

CHARACTER PKKS3D*13, PKKM1S*13, INFILC*13 / INFILR*13 
CHARACTER INFILP*13, INFILA*13, 0UTFIL*13 



LOGICAL* 1 POSREC , VELREC , ACCREC , ENDPOSDAT , ENDVELDAT , 
* ENDACCDAT , ENDRLLDAT 



**** INPUT DATA, DESIGNATE FILENAMES **** 



OPEN ( 1 , FI LE= 
OPEN (2, FILE= 
OPEN (3, FILE= 
OPEN (4, FILE= 
OPEN ( 11 , FILE= 



' DATA\POSITION . OBS ' , 
' DATA\VELOCITY . OBS ' , 
' DATA\ACCELERA . OBS ' , 
' DATA\ROLL_YAW . OBS ' , 
' DATA\OBSERVAT . DAT ' , 



STATUS= 

STATUS= 

STATUS= 

STATUS= 

STATUS= 



'OLD' ) 
' OLD ' ) 
'OLD' ) 
' OLD ' ) 
' NEW ' ) 



**** NOTIFY USER OF MAXIMUM NUMBER OF DATA POINTS **** 



WRITE (*,770) MAXOBS 

770 FORMAT(' The MAXIMUM number of points that can be ', 

* 'analyzed is: ',14) 

WRITE ( * , 771) 

771 FORMAT ( ' To use more, you must reset the parameter ', 

* 'MAXOBS',/,' in the main program and recompile') 



*************** SECTION 2 ************************** 

**** GET STARTING TIME FOR RANGE DATA **** 

READ ( 1, 750, END=1200) PC , X, Y , Z , HH , MM, SS 
WRITE (* , 751) HH,MM, SS 

TORANGE = 3600. *REAL (HH) +60 . *REAL (MM) +SS 
WRITE ( * , 752 ) 

READ (* , 755) HOUR, ASK, MIN, SEC 
IF (ASK .EQ. ' ') GOTO 754 

TORANGE = 3600 . *REAL(HOUR) +60 . *REAL(MIN) +SEC 
WRITE ( * , 756) HOUR, MIN, SEC 

754 CONTINUE 
C 

750 FORMAT (IX, 16, 2X,3F10.1, 10X, 12 , IX, 12 , IX, F5 . 2 ) 

751 FORMAT (/, ' The first position time 
*is: • ,12, • : ' ,12, ' : ' ,F7.4) 

752 FORMAT (/, ' If you do not wish to change it, press 

*"Enter" ' ,/ , ' If you wish to change it, enter the new 
*time in the format: ',/,' HH:MM:SS . SSSS ' ) 

755 FORMAT (12, A1 ,12, IX , F7 . 4 ) 

756 FORMAT ( ' The first position time is 
*now: ' , 12 , ' : ' , 12 , ' : ' , F7 . 4 ) 
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**** GET STARTING TIME FOR TORPEDO DATA **** 

READ (2,760, END=1201) MMV , SSV, XVEL, YVEL, ZVEL 
HHV = 0 

TOTORP = 60 . *REAL(MMV) +SSV 
WRITE ( * , 761 ) HHV, MMV, SSV 
WRITE (*,752) 

READ (*, 755, END=763) HOUR , ASK, MIN , SEC 
IF (ASK .EQ. ' ') GOTO 763 

TOTORP = 3600 . *REAL(HOUR) +60 . *REAL(MIN) +SEC 
WRITE (*,762) HOUR, MIN, SEC 
763 CONTINUE 
C 

760 FORMAT ( IX , 12 , IX , F7 . 4 , 10X , F9 . 3 , 5X , F9 . 3 , 5X , F9 . 3 ) 

761 FORMAT (/, ' The first velocity time 
*is: ' ,12, ' : ' ,12, • : ' ,F7.4) 

762 FORMAT ( 1 The first velocity time is 
*now: • ,12, ' : • ,12, • : ' ,F7.4) 

C 

C 

C **** SELECT THE FIRST RECORD FROM EACH DATA SET THAT **** 
C **** REPRESENTS AN OBSERVATION AT OR AFTER THE STARTING **** 



C **** TIME. NOTE THAT WE MUST USE A SLIGHTLY DIFFERENT **** 
c **** PROCEDURE FOR THE POSITION AND VELOCITY DATA **** 
C **** FILES, SINCE WE HAVE ALREADY READ A RECORD FROM **** 
C **** EACH OF THEM. **** 



C 

201 CONTINUE 

IF ( PTM .GE. TORANGE ) GOTO 202 
READ (1,750, END=12 00) PC , X , Y , Z , HH, MM , SS 
PTM = 3600 . *REAL(HH) +60. *REAL(MM) +SS 
GOTO 201 
C 

202 CONTINUE 

IF ( VTM .GE. TOTORP ) GOTO 203 
READ (2,760, END=1201) MMV, SSV, XVEL, YVEL, ZVEL 
VTM = 60 . *REAL (MMV) +SSV 
GOTO 202 
C 

203 CONTINUE 

READ (3,760, END=12 02 ) MMA, SSA, ACCX , ACCY , ACCZ 
ATM = 60 . *REAL(MMA) +SSA 
IF ( ATM .LT. TOTORP ) GOTO 203 
ATM 1= ATM 
ACCXM1=ACCX 
ACCYM1=ACCY 
ACCZM1=ACCZ 
C 

204 CONTINUE 

READ (4 , 760 , END=1203 ) MMR, SSR, EYAW, EROLL, EPITCH 
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RTM = 60. *REAL(MMR)+SSR 
IF ( RTM . LT. TOTORP ) GOTO 204 
RTM1=RTM 
RLLM1=ER0LL 



C 

C 

C **** 

C 

PTM 

VTM 

ATM 

RTM 



SYNCHRONIZE RANGE AND TORPEDO CLOCKS ****** 

PTM - TORANGE 
VTM - TOTORP 
ATM - TOTORP 
RTM - TOTORP 



C 

C**************** SECTION 3 ************************* 

C 

C*** BUILD THE OBSERVATION ARRAY BY SEQUENTIALLY *** 

C*** DETERMINING THE NEXT OBSERVATION TIME AND THE *** 

C*** SPECIFIC FIELDS OBSERVED AT THAT TIME *** 



C 

C 

WRITE ( * , * ) ' ' 

WRITE (*,*) 'Reading Data Files' 
NOBSERV = 0 
ENDPOSDAT = .FALSE. 

ENDVELDAT = .FALSE. 

ENDACGDAT = .FALSE. 

ENDRLLDAT = .FALSE. 



C 



300 CONTINUE 

IF (ENDVELDAT .AND. ENDPOSDAT ) GOTO 400 
IF ( NOBSERV. GE.MAXOBS) GOTO 395 
C 

NOBSERV = NOBSERV + 1 
POSREC = .FALSE. 

VELREC = .FALSE. 

ACCREC = .FALSE. 

TNEXTOBS = DMIN1 (PTM, VTM) 

IF (ENDPOSDAT) TNEXTOBS = VTM 

IF (ENDVELDAT) TNEXTOBS = PTM 

VTIME (NOBSERV) = TNEXTOBS 
C 



C*** SEE IF THERE'S A POSITION OBSERVATION AT THIS TIME *** 
C 

311 CONTINUE 

IF ( ( (PTM-TNEXTOBS) .LT. DEPS ) .AND. 

* ( .NOT. (ENDPOSDAT) ) ) THEN 

POSREC = .TRUE. 

PTC (NOBSERV) =PC 
ZK(1, NOBSERV) =X 
ZK( 4, NOBSERV) =Y 
ZK( 7, NOBSERV) =Z 
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READ (1,750, END=3 12 ) PC , X , Y , Z , HH , MM, SS 
PTH = 3600 . *REAL(HH) +60 . *REAL(MM) +SS - TORANGE 
IF ( PTM .LT. TNEXTOBS ) GOTO 1210 
ENDIF 
GOTO 321 

312 ENDPOSDAT= . TRUE . 

WRITE (*,313) NOBSERV 

313 FORMAT (' End of Position Record File Encountered at 

* 'Observation ',14) 



*** SEE IF THERE'S A VELOCITY OBSERVATION AT THIS TIME * 

321 CONTINUE 

IF ( ( ( VTM-TNEXTOBS ) .LT. DEPS ) .AND. 

* ( .NOT. (ENDVELDAT) ) ) THEN 

VELREC = .TRUE. 

ZK ( 2 , NOBSERV) = 3 . *XVEL 
ZK ( 5 , NOBSERV) =-3 . * YVEL 
ZK ( 8 , NOBSERV) =-3 . *ZVEL 

READ (2,760, END=3 22) MMV, SSV, XVEL, YVEL, ZVEL 
VTM = 60 . *REAL (MMV) +SSV - TOTORP 
IF ( VTM .LT. TNEXTOBS ) GOTO 1211 
ENDIF 
GOTO 331 

322 ENDVELDAT= . TRUE . 

WRITE (*,323) NOBSERV 

323 FORMAT (' End of Velocity Record File Encountered at 

* 'Observation ',14) 



C* SEE IF THERE'S AN ACCELERATION OBSERVATION AT THIS TIME 
C* THEN USE THE CLOSEST ACCELERATION OBSERVATION TO THE 
C* CURRENT TIME. 

C 

331 CONTINUE 

IF ( ( ATM .LE. TNEXTOBS ) .AND. 

* ( .NOT. (ENDACCDAT) ) ) THEN 

ATM 1= ATM 
ACCXM1=ACCX 
ACCYM1=ACCY 
ACCZM1=ACCZ 

332 READ (3,760, END=33 3 ) MMA, SSA, ACCX , ACCY , ACCZ 
ATM = 60. *REAL(MMA)+SSA - TOTORP 

IF ( ATM .LE. TNEXTOBS ) GOTO 332 
ENDIF 
GOTO 335 



333 ENDACCDAT= . TRUE . 
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WRITE (*,334) NOBSERV 

334 FORMAT (' End of Acceleration Record File Encountered at 
♦Observation ',14) 

C 

335 CONTINUE 

IF ( (TNEXT0BS-ATM1) .LT. (ATM - TNEXTOBS) ) THEN 
ZK ( 3 , NOBSERV) =ACCXM1 
ZK ( 6 , NOBSERV) =ACCYM1 
ZK ( 9 , NOBSERV) =ACCZM1 
ELSE 

ZK ( 3 , NOBSERV) =ACCX 
ZK ( 6 , NOBSERV) =ACCY 
ZK ( 9 , NOBSERV) =ACCZ 

ENDIF 

SEE IF THERE'S A ROLL OBSERVATION AT THIS TIME ***** 
THEN USE THE CLOSEST ROLL OBSERVATION TO THE ***** 
CURRENT TIME. ***** 



341 CONTINUE 

IF ( ( RTM .LE. TNEXTOBS ) .AND. 

* ( .NOT. (ENDRLLDAT) ) ) THEN 

RTM1=RTM 

RLLMl=EROLL 

342 READ (4 , 160 , END=343) MMR, SSR, EYAW, EROLL, EPITCH 
RTM = 60 . *REAL (MMR) +SSR - TOTORP 

IF ( RTM .LT. TNEXTOBS ) GOTO 342 
ENDIF 
GOTO 345 

343 ENDRLLDAT= . TRUE . 

WRITE (*,344) NOBSERV 

344 FORMAT (' End of Roll Record File Encountered at ', 

* 'Observation ',14) 

C 

345 CONTINUE 

IF ( (TNEXTOBS-RTM1) .LT. (RTM - TNEXTOBS) ) THEN 
ROLL ( NOBSERV) =RLLM1 
ELSE 

ROLL (NOBSERV) =EROLL 

ENDIF 

ACCREC = .TRUE. 

C 

C**** CODE THE SOURCE (S) OF THIS RECORD ****** 

C 

351 CONTINUE 
SRC = 0 

IF ( POSREC ) SRC = 73 

IF ( VELREC ) SRC = SRC +146 

C IF ( ACCREC ) SRC = SRC + 292 

SOURCE (NOBSERV) = SRC 



C 

C*** 

C*** 

C*** 
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GOTO 300 

395 WRITE ( * , 396 ) NOBSERV , MAXOBS 

396 FORMAT (' The total number of observations (',14,') 
♦exceeds allocate storage (MAXOBS = ' , 14 ,/ , 5x, ' Some data 
♦will be lost. You may wish to recompile the program',/, 
* with a higher value of MAXOBS. (Enter "Y" to 
♦continue) ' ) 

READ (*,397) ASK 

397 FORMAT (Al) 

IF ( ( ASK .NE. ’Y' ) .AND. ( ASK .NE. 'y' ) ) STOP 



***************** SECTION 4 ************************** 

**** WRITE OUT DATA FILES ♦♦♦♦ 



400 CONTINUE 
CLOSE (1) 

CLOSE (2) 

CLOSE (3) 

CLOSE (4) 

WRITE (*,902) NOBSERV 
WRITE (11, 901) NOBSERV 
DO 401 IPRT = 1, NOBSERV 

WRITE (11,900) PTC ( IPRT) ,VTIME(IPRT) , SOURCE ( IPRT) , 

* ( ZK(JPRT, IPRT) ,JPRT=1,MAXSTATE ), ROLL (IPRT) 

401 CONTINUE 

900 FORMAT (13 (1X,E14 . 8) ) 

902 FORMAT (/, ' Writing ',15,' Combined Observations to 
♦File' ) 

901 FORMAT ( ' OUTPUT FROM PROGRAM OBSDATA. FOR ',/, 

♦IX, 14 , ' OBSERVATIONS ' , 

*/,7X, ' PTC ' , 11X, 'TIME ' , 10X, ’ SOURCE ', 10X , 'ZK(l) ',10X, 

* ' ZK (2 ) ',10X, ' ZK ( 3 ) ',10X, 'ZK(4) ',10X, 'ZK(5) ',10X, 'ZK(6) ', 
♦10X, ' ZK (7 ) ' , 10X, ' ZK(8) ' , 10X, 'ZK(9) ’ , 10X, 'ROLL' ) 

CLOSE ( 11 ) 



STOP 

**** ERROR ROUTINES FOR UNEXPECTED END OF DATA FILE **** 

1200 WRITE(*, 1300) 

1300 FORMAT ( ' **** ERROR - RANGE DATA FILE ENDED BEFORE A 
♦VALID ', 'RECORD COULD BE LOCATED') 

STOP 

C 

1201 WRITE(*, 1301) 
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1301 FORMAT ('**** ERROR - VELOCITY DATA FILE ENDED BEFORE A 
♦VALID RECORD COULD BE LOCATED') 

STOP 



C 

1202 WRITE(*, 1302) 

1302 FORMAT ('**** ERROR - ACCELERATION DATA FILE ENDED BEFORE 
*A VALID RECORD COULD BE LOCATED') 

STOP 



C 

1203 WRITE (* , 1303 ) 

1303 FORMAT ('**** ERROR - ROLL DATA FILE ENDED BEFORE A VALID 
♦RECORD COULD BE LOCATED') 

STOP 



1210 WRITE(*, 1310) 

1310 FORMAT ( ' **** ERROR - RANGE DATA FILE RECORD OUT OF VALID 
♦TIME ORDER ' ) 

STOP 



C 

1211 WRITE(*, 1311) 

1311 FORMAT ( ' **** ERROR - VELOCITY DATA FILE OUT OF VALID 
♦TIME ORDER') 

STOP 



C 

1212 WRITE(*, 1312) 

1312 FORMAT ('**** ERROR - ACCELERATION DATA FILE OUT OF VALID 
♦TIME ORDER') 

STOP 



C 

1213 WRITE(*, 1313) 

1313 FORMAT ('**** ERROR - ROLL DATA FILE OUT OF VALID 
♦TIME ORDER') 

STOP 



C 



END 



on no on noon on on on on 



B 



POSCONV . FOR 



C 

C POSCONV. FOR 

C 

C THIS PROGRAM CONVERTS THE DATA FILES CREATED BY THE 
C MATLAB SIMULATION FOR POSITION, VELOCITY, AND 

C ACCELERATION INTO THE PROPER FORMAT FOR USE BY THE KALMAN 
C FILTER PROGRAM. IT ALSO CREATES A ROLL ANGLE DATA FILE 

C OF 0 DEGREES THROUGHOUT. 

C 

INTEGER PC, MIN, HR 

DOUBLE PRECISION SEC, X, Y , Z , VX, VY , VZ , AX , AY, AZ , YAW, 
DOUBLE TRECISION ROLL, PITCH, TOTSEC, DELTAT 
LOGICAL*! ENDPOSREC, ENDVELREC, ENDACCREC 



******* OPEN THE TIMING INFORMATION DATA FILE *********** 
OPEN ( 1 , FILE= ' DATA\DELTSIMU ' , STATUS= ' OLD ' ) 

******* OPEN THE INPUT POSITION DATA FILE *********** 

OPEN ( 2 , FILE= ' DATA\SIMULPOS ' , STATUS= ' OLD ' ) 

****** OPEN THE INPUT VELOCITY DATA FILE *********** 

OPEN ( 3 , FILE= ' DATA\SIMULVEL ' , STATUS= ' OLD ' ) 



****** OPEN THE INPUT ACCELERATION DATA FILE *********** 
OPEN ( 4 , FILE= ' DATA\SIMULACC ' , STATUS= ' OLD ' ) 



****** OPEN THE OUTPUT POSITION DATA FILE *********** 

OPEN ( 12 , FILE= • DATA\POSITION . OBS ' , STATUS= ' NEW ' ) 

****** OPEN THE OUTPUT VELOCITY DATA FILE *********** 

OPEN ( 13 , FILE= ' DATA\VELOCITY . OBS ' , STATUS= ' NEW ' ) 

****** OPEN THE OUTPUT ACCELERATION DATA FILE *********** 
OPEN ( 14 , FILE= ' DATA \ACCELERA. OBS ' , STATUS= ' NEW ' ) 

****** OPEN THE OUTPUT ROLL ANGLE DATA FILE *********** 
OPEN ( 15 , FILE= ' DATA\ROLL_YAW.OBS 1 , STATUS= ' NEW ' ) 

C 

C 

C *********************************************************** 
C *** INPUT THE MATLAB FILES AND CONVERT TO INPUT **** 

C *** FORMAT FOR THE FILTER PROGRAM **** 

C *********************************************************** 

C 

PC=1 
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TOTSEC= 0 . DO 
READ (1,20) DELTAT 



ENDPOSREC = .FALSE. 

ENDVELREC = .FALSE. 

ENDACCREC = .FALSE. 

100 CONTINUE 

IF ( ENDPOSREC .AND. ENDVELREC .AND. ENDACCREC ) 

* GOTO 200 

*** ** UPDATE TIMING DATA *** 

SEC = AMOD (TOTSEC ,60.0) 

MIN = MOD ( DINT (TOTSEC/60 . DO ) , 60 ) 

HR = DINT (TOTSEC/ 3 600 . DO ) 

*** PROCESS ROLL, YAW AND PITCH DATA *** 

YAW= . 00 
ROLL=. 00 
PITCH= . 00 

WRITE (15,40) MIN , SEC , YAW , ROLL , PITCH 

*** PROCESS POSITION DATA *** 

IF ( .NOT. ENDPOSREC ) THEN 
READ (2,20, END=101) X,Y,Z 
WRITE (12,30) PC , X , Y , Z , HR , MIN , SEC 
ENDIF 

*** PROCESS VELOCITY DATA *** 

IF ( .NOT. ENDVELREC ) THEN 
READ (3,20, END=102 ) VX,VY,VZ 
VY=-VY 
VZ=-VZ 

WRITE (13,40) MIN , SEC , VX , VY , VZ 
ENDIF 

*** PROCESS ACCELERATION DATA *** 

IF ( .NOT. ENDACCREC ) THEN 
READ (4,20, END= 103) AX,AY,AZ 
WRITE (14,40) MIN, SEC, AX, AY, AZ 
ENDIF 

*** UPDATE COUNTERS AND TIMING *** 

PC=PC+1 

TOTSEC=TOTSEC+DELTAT 
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c 



GOTO 100 



20 

30 

40 



FORMAT (3 (IX, E15. 7) ) 

FORMAT ( IX, I6,2X,3F10.1, 10X, 12 , IX, 12 , IX, F5 . 2 ) 
FORMAT (IX, 12 , IX, F7 . 4 , 10X, F9 . 3 , 5X, F9 . 3 , 5X, F9 . 3) 



C 



101 CONTINUE 

WRITE (*,121) PC-1 
ENDPOSREC = .TRUE. 

GOTO 100 

121 FORMAT ( • End of Data After ',14,' Position Records 
*Read ' ) 



102 CONTINUE 

WRITE (* , 122 ) PC-1 
ENDVELREC = .TRUE. 

GOTO 100 

122 FORMAT ( • End of Data After ',14,' Velocity Records 
*Read • ) 



103 CONTINUE 

WRITE (*,123) PC-1 
ENDACCREC = .TRUE. 

GOTO 100 

123 FORMAT ( ' End of Data After ',14,' Acceleration Records 
*Read ' ) 

C 

200 CONTINUE 
CLOSE (1) 

CLOSE (2) 

CLOSE (3) 

CLOSE ( 4 ) 

CLOSE (12) 

CLOSE ( 13 ) 

CLOSE (14) 

CLOSE (15) 

STOP 



C 



C 



END 
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TORPMAN . FOR 



C — KALMAN FILTER KALMAN SMOOTHING ALGORITHM 

C — THIS PROGRAM IS DESIGNED TO PROCESS 3/D DATA FILES FROM 
C — THE UNDERSEA TRACKING RANGE AT KEYPORT WA. A KALMAN 
C — FILTER IS APPLIED TO THE TRACK DATA WHICH CONSISTS OF 
C — X , Y , AND Z COORDINATES, AS WELL AS VELOCITY COMPONENT IN 
C — X, Y , AND Z COORDINATES. THEN, A KALMAN FILTER SMOOTHING 
C — ROUTINE GENERATES SMOOTHED POINTS IN X,Y, AND Z. THE 
C — PROGRAM GENERATES OUTPUT FILES WHICH CONTAIN THE VARIANCE 
C — OF THE X ESTIMATE VS SAMPLE FOR BOTH THE FORWARD KALMAN 
C — FILTER CASE AND THE KALMAN SMOOTHED CASE. FILES ARE ALSO 
C — GENERATED WHICH CONTAIN THE FILTERED X, Y, AND Z ESTIMATES 
C— AND THE SMOOTHED X,Y, AND Z ESTIMATES. THIS PROGRAM IS 
C — DESIGNED TO RUN ON THE IBM/AT PERSONAL COMPUTER BUT DUE 
C— TO THE SIZE OF THE DATA SETS INVOLVED, PLOTTING CANNOT BE 
C — DONE WITH THIS PROGRAM. PLOTTING OF OUTPUT DATA IS 
C— DONE USING MATLAB. THE PROGRAM GIVES THE USER THE OPTION 
C — OF CHANGING THE STARTING TIMES FOR BOTH THE RANGE AND 
C — VELOCITY DATA, THE VALUE OF THE INITIAL COVARIANCE 
C — MATRIX, AND THE EXCITATION PROCESS VECTOR. THE USER IS 
C — ALSO REQUIRED TO PROVIDE THE NAMES OF THE INPUT AND 
C — OUTPUT DATA FILES, AND THE NUMBER OF POINTS TO CALCULATE 
C — WITH A MAXIMUM OF 2500. THE PROGRAM USES A LARGE AMOUNT 
C— OF HARD DISK SPACE FOR TEMPORARY FILES (APPROXIMATELY 
C — 160K BYTES/100 POINTS, 4 MEG BYTES MAX FOR 2500 POINTS). 

C 

C*************************** SECTION 1 ********************* 
C 

c *************** DECLARATION OF PARAMETERS *********** 
C 

INTEGER ONE 

PARAMETER ( ONE=l, MAXOBS=250 , NDIMEN=3 ) 

PARAMETER ( NUMSTA=3 *NDIMEN , NSTMAT=NUMSTA*NUMSTA, 

* NOBVEC=NUMSTA*MAXOBS ) 

C 

C *************** DECLARATION OF ARRAYS ************ 

C 

DOUBLE PRECISION XKK (NUMSTA) , XKKM1 (NUMSTA) , 

* ZZ (NUMSTA) ZKKM1 (NUMSTA) , XNNM1 (NUMSTA) , XP1 (NUMSTA) 

DOUBLE PRECISION TMPV1 (NUMSTA) , TMPV2 (NUMSTA) 

DOUBLE PRECISION VTIME (MAXOBS ), SOURCE (MAXOBS ) , 

* ROLL (MAXOBS) 

DOUBLE PRECISION XKKS (NUMSTA, MAXOBS) 

C 



61 



o o o 



DOUBLE PRECISION PHI (NUMSTA, NUMSTA) , 

* Q (NUMSTA, NUMSTA) , HI (NUMSTA, NUMSTA) , 

* PKKM1 (NUMSTA, NUMSTA) , PKK( NUMSTA, NUMSTA) , 

* H (NUMSTA, NUMSTA) ,HTRANS (NUMSTA, NUMSTA) , 

* R (NUMSTA, NUMSTA) ,G (NUMSTA, NUMSTA) , 

* PHIT (NUMSTA, NUMSTA) , PKKS (NUMSTA, NUMSTA) , 

* PKKP1 (NUMSTA, NUMSTA) ,AK( NUMSTA, NUMSTA) , 

* RPROJ (NUMSTA, NUMSTA) 

DOUBLE PRECISION TEMPI (NUMSTA, NUMSTA) , 

* TEMP2 (NUMSTA, NUMSTA) ,TEMP3 (NUMSTA, NUMSTA) 

*********** DECLARATION OF SCALARS *************** 



C 

C 

C 

C 

C 

C 

C 

C 

c 



c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



DOUBLE PRECISION ACCTHRSH , ROLTHRSH , WTMIN , WTMAX 
COMMON /CBLK/ PTC (MAXOBS ) , XYRANGE ( 3 , 2 ) , 

* PKKONEONE (MAXOBS ) , IR1 

INTEGER KF, SRC 

CHARACTER PKKS3D*13, PKKM1S*13 

CHARACTER ASK*1, INFILC*13, INFILR*13, INFILP*13, 

* INFILA*13 , OUTFIL*13 



******* INITIALIZATION OF SELECTED ARRAYS ************ 
****** (THOSE NOT OTHERWISE INITIALIZED) ************* 

DATA HI /NSTMAT*0 . DO/ , VTIME /MAXOBS*0 . DO/ , 

* XKKS /NOBVEC*0 . DO/ , SOURCE /MAXOBS*0 . DO/ , 

* PKKM1 /NSTMAT*0 . DO/ , ROLL /MAXOBS*0 . DO/ 

********************** SECTION 2 ************************* 



***** DESIGNATE AND OPEN INPUT AND OUTPUT FILES ******** 



WRITE (*,*) 'ENTER NAME OF INPUT RANGE POSITION DATA 
*FILE ' 

READ (50, ' (A) ' ) INFILP 

WRITE (*,*) 'ENTER NAME OF INPUT INTERNAL VELOCITY DATA 
♦FILE ' 



READ (51 , ' (A) ' ) INFILA 

WRITE (*,*) 'ENTER NAME OF INPUT INTERNAL ACCELERATION 
♦DATA FILE' 

READ (52, ' (A) ' ) INFILC 

WRITE (*,*) 'ENTER NAME OF INPUT ROLL ANGLE DATA FILE' 
READ (53, ' (A) ' ) INFILR 

WRITE (*,*) 'ENTER NAME OF SMOOTHED DATA FILE' 

READ (54, ' (A) ' ) OUTFIL 

OPEN ( 2 , FILE* 'DATA\XKKFWD.DAT', STATUS* ' NEW ' ) 

OPEN ( 3 , FILE= ' DATA\XKSMOOTH . DAT ' , STATUS= ' NEW ' ) 

OPEN ( 4 , FILE= 'DATA\PKKOUT.DAT', STATUS* • NEW ' ) 
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OPEN ( 11 , FILE= 1 DATA\OBSERVAT . DAT ' , STATUS= ' OLD • ) 

PKKS3D = ' DATA\$3DTEMP 1 
PKKM1S = ' DATA\$M1TEMP ' 

OPEN ( 63 , FILE=PKKS3D, ACCESS= • DIRECT ' , 

* STATUS= 'NEW' , FORM= ' FORMATTED ' ,RECL=13 5) 

OPEN ( 61 , FILE=PKKM1S , ACCESS= ' DIRECT ' , 

* STATUS^ 'NEW' ,FORM=' FORMATTED' ,RECL=13 5) 

C 

C ********* INITIALIZE SELECTED COUNTERS ************* 

C 

IR1 = 1 
IR = 1 
K =0 
C 

c **** INITIALIZE ARRAY DIMENSIONS AND VARIABLES ****** 

C 

L = 1 
N = 9 
NW = 3 
ND = NUMSTA 
LD = ONE 
C 

PKKM1 (1,1) = 1000000. 0D0 
ROLTHRSH=5 . 0 
ACCTHRSH=5 . 0 
WTMIN=1800 . 

WTMAX = 20000.0 
C 

c ***** INITIALIZE THE IDENTITY MATRIX AND X(l|0) ****** 

C 

DO 190 I = 1, 9 
XKKMl(I) =0.0 
HI (I , I) = 1. 

190 CONTINUE 
C 

c ***** ECHO initial filter parameters and allow ***** 

c ***** USER TO CHANGE THEM IF DESIRED. ***** 

C 

CALL CHANGE ( PKKM1 , WTMIN , WTMAX, N) 

CALL SETTHRSH (ROLTHRSH , ACCTHRSH) 

C 

C 

q ********************** SECTION 3 ************************* 

c 

c **** READ IN THE ''RAW'' OBSERVATION DATA FILE, WHICH *** 

C **** HAS BEEN PROPERLY FORMATTED IN AN EARLIER *** 

C **** E.G. OBS DATA. FOR. WHEN THIS DATA IS READ IN *** 

C **** THE Kth OBSERVATION IS ORIGINALLY LOADED INTO *** 

C **** XKKS ( * , K) . THEN, WHEN THE FILTER IS RUN, THIS *** 

C **** VALUE WILL BE REPLACED WITH THE ESTIMATED STATE *** 

C **** X(KjK). FINALLY, WHEN THE BACKWARD SMOOTHING IS *** 
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C **** RUN, THIS VALUE WILL BE REPLACED WITH THE FINAL *** 
C ***** SMOOTHED VALUE. *** 

C 

READ (11,210) IR1 
IF ( IR1 .GT. MAXOBS ) THEN 
WRITE (*,209) IR1, MAXOBS 
STOP 
ENDIF 
C 

DO 201 IPRT = 1 , IR1 

READ (11,211, END=300) PTC(IPRT) ,VTIME(IPRT) , 

* SOURCE (IPRT) , ( XKKS ( JPRT , IPRT) ,JPRT=1,9) , 

* ROLL (IPRT) 

201 CONTINUE 

CLOSE (11) 

C 

209 FORMAT ( ' The Number of Observations ',15,', exceeds 
♦the',' maximum storage available (',15,') ' ) 

210 FORMAT (/, IX, 14 ,/) 

211 FORMAT (13 (1X,E14.8) ) 

300 CONTINUE 



******************** SECTION 4 ************************** 

********************************************************* 

**** IMPLEMENT THE (FORWARD) KALMAN FILTER ****** 

********************************************************* 

100 K = K + 1 

WRITE (*,*) 'Forward Filter - Step',K 

*** BYPASS COMPUTATION OF P(l|0) AND X(lj0) *** 

*** DURING FIRST ITERATION *** 

IF ( K .EQ. 1 ) GO TO 101 

*** GENERATE THE APPROPRIATE STATE TRANSITION *** 
*** MATRIX ( PHI ) AND ITS TRANSPOSE, BASED ON *** 
*** THE ELAPSED TIME SINCE THE LAST OBSERVATION.*** 

DT = VTIME (K) -VTIME (K-l) 

CALL PHIDEL(DT,NW, PHI ,ND) 

CALL TRANS ( PHI, N, N , PHIT, ND, ND) 

**** DETERMINE WHETHER THE FILTER BELIEVES THE **** 
**** TORPEDO IS MANEUVERING, AND GENERATE THE **** 
**** APPROPRIATE COVARIANCE OF EXCITATION MATRIX **** 

CALL MANDET (ROLL, XKKS , K , ROLTHRSH , ACCTHRSH , WTMIN , 

* WTMAX , DT , Q , ND , NW) 
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** * 



*** 



c 

c 

c 

c 

c 

c 

c 

c 



c 

c 

c 

c 

c 

c 



FORM P (K [ K-l ) , BASED ON THE EQUATION 



P(KjK-l) = PHI*P(K-1 J K-l) *TRANS { PHI } + Q (K) 

*** _ WHERE P (K-l j K-l) IS P (K j K) FROM THE *** 

*** PREVIOUS STEP. *** 

CALL PROD ( PKK , PHIT ,N,N,N, TEMPI , ND , ND , ND) 

CALL PROD ( PHI , TEMPI , N , N, N, TEMP2 , ND , ND , ND) 

CALL ADD(TEMP2 ,Q,N,N, PKKM1,ND / ND) 



*** AND ALSO X(KjK-l) = PHI*X(K-1 j K-l) *** 
*** ( WHERE X (K-l J K-l) IS X(K|K) FROM THE *** 
*** PREVIOUS STEP.) *** 

CALL PROD ( PHI , XKK ,N,N, ONE , XKKM1 , ND , ND , ONE ) 



101 CONTINUE 



**** DETERMINE THE SOURCE OF THIS OBSERVATION **** 
**** (E.G. POSITION ONLY, VELOCITY ONLY, **** 
**** POSITION AND VELOCITY, ETC.) AND FORM THE **** 
**** -CORRESPONDING MEASUREMENT MATRIX ( H (K) ) **** 
**** AND ITS TRANSPOSE **** 



SRC = SOURCE (K) 

CALL BUILDH ( SRC , M , H , NW , ND) 

CALL TRANS (H, M, N, HTRANS , ND, ND) 

*** DETERMINE THE FULL (RANGE -DEPENDENT) *** 

*** COVARIANCE OF MEASUREMENT NOISE MATRIX ( R ) *** 
*** THEN PROJECT IT ONTO THE CURRENT OBSERVATION *** 
*** VECTOR COMPONENTS *** 

CALL BUILDR (R, NW, ND) 

CALL PROD (H,R,M,N,N, TEMPI, ND,ND,ND) 

CALL PROD ( TEMPI , HTRANS ,M,N,M, RPROJ , ND , ND , ND) 

**** USE THE MEASUREMENT MATRIX ( H ) TO SELECT **** 
**** THE CORRECT COMPONENTS ( Z(K) ) FROM THE **** 
**** "RAW" OBSERVATION ARRAY ( XKKS ) . **** 

CALL PROD (H, XKKS (1,K) ,M, ND, ONE , ZZ , ND, ND, ONE) 

**** COMPUTE THE OPTIMUM GAIN MATRIX G (K) **** 

**** BASED ON THE FORMULA **** 

G (K) = 
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P(KjK-l) *TRANS{H(K) }*INV{ [H (K) *P (K | K-l ) *TRANS { H (K) } + R] } 

CALL PROD ( PKKM1 , HTRANS ,N,N,M, TEMPI , ND , ND , ND) 

CALL PROD ( H , TEMPI ,M,N,M, TEMP2 f ND f ND, ND) 

CALL ADD ( TEMP2 , RPRO J ,M,M, TEMP3 , ND , ND ) 

CALL RECIP(TEMP3 ,TEMP2 ,M,ND) 

CALL PROD ( TEMPI , TEMP2 , N , M , M f G f ND , ND , ND) 



**** CALCULATE Z(K|K-1) BASED ON THE FORMULA **** 
**** Z (K j K-l) = H(K) *X(K| K-l) **** 

CALL PROD ( H , XKKM1 , N, N, ONE , ZKKM1 , ND , ND , ONE) 

**** SOLVE FOR THE UPDATED STATE ESTIMATE VECTOR **** 
**** ( X (K | K) ), BASED ON THE EQUATION **** 

**** X (K j K) = X(KjK-l) + G (K) * [ Z ( K) - Z(KjK-l)] **** 
**** WHERE Z (K) IS THE CURRENT OBSERVATION **** 



CALL SUB ( ZZ , ZKKM1 , M , ONE f TMPV1 , ND , ONE) 

CALL PROD (G,TMPV1,N,M, ONE, TMPV2,ND,ND, ONE) 

CALL ADD ( XKKM1 , TMPV2 , N , ONE , XKK , ND , ONE ) 

*** NOW COMPUTE THE COVARIANCE OR ERROR ESTIMATE *** 
*** MATRIX P(KjK) , BASED ON THE EQUATION: *** 

P ( K | K) = { I - G(K)*H(K) ) *P(Kj K-l) 

CALL PROD (G,H,N,M,N, TEMPI, ND,ND,ND) 

CALL SUB (HI , TEMPI ,N,N, TEMP2 , ND, ND) 

CALL PROD (TEMP2 , PKKM1 ,N,N,N, PKK , ND , ND , ND) 

*** STORE THE FILTERED DATA IN XKKS(*,K) (FOR *** 
*** SUBSEQUENT SMOOTHING (BACKWARDS FILTERING) *** 
*** AND ALSO SAVE X(K|K), P(K]K), AND P(K|K-1) *** 

DO 50 1=1,9 

50 XKKS ( I , K) = XKK ( I ) 

WRITE ( 2 , 602 ) VTIME (K) , XKKS ( 1 , K) , XKKS(4,K), XKKS(7,K) 
WRITE (4, 900) PKK (1,1) , PKK (1,4) , PKK (4,1) , PKK (4, 4) 

KREC = 9* (K-l) +1 

WRITE (61,800, REC=KREC) PKKM1 

WRITE (63,800, REC=KREC) PKK 

602 FORMAT ( 4 ( IX , E14 . 8 ) ) 

800 FORMAT ( 9 ( IX , D14 . 8 ) ) 

900 FORMAT (9 (1X,E14 .8) ) 

**** AND CONTINUE UNTIL ALL OBSERVATIONS ARE FILTERED **** 
IF (K.LT.IR1) GOTO 100 
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CLOSE (2) 
CLOSE (4) 
CLOSE (61) 
CLOSE (63) 



*********************** SECTION 5 ************************ 

********************************************************** 

* IMPLEMENT THE SMOOTHING (BACKWARD FILTERING) ROUTINE * 
********************************************************** 



***** OPEN THE REQUIRED DATA FILES, AND INITIALIZE **** 
***** THE SMOOTHED COVARIANCE OF ERROR ESTIMATE **** 

***** MATRIX TO P(IRljlRl) **** 

OPEN ( 63 ,FILE=PKKS3D,ACCESS=' DIRECT' , STATUS= ' OLD ' , 

* FORM= ' FORMATTED ' ,RECL=13 5) 

OPEN ( 6 1,FILE=PKKM1S,ACCESS=' DIRECT' , STATUS= ' OLD ' , 

* FORM= ' FORMATTED ' ,RECL=13 5) 

KF=9* ( IR1-1) +1 

READ ( 63 , 800 , REC=KF) PKKS 

******* THEN BEGIN THE BACKWARD SMOOTHING ******** 

DO 600 K=1 , IR1 - 1 

KI= IR1 - K - 

WRITE (*,*) 'Backward Smoothing - Step ' , KI 

***** GENERATE PHI MATRIX AND RETRIEVE THE STORED *** 

***** VALUES OF P (K, K) , AND P(K+l|K) *** 

DT = VTIME (KI+1) - VTIME(KI) 

CALL PHIDEL(DT, NW, PHI , ND) 



KF= 1+9* (KI-1 ) 

READ (63,800, REC=KF ) PKKS 

READ (61,800, REC=KF+9 ) PKKP1 

**** NOW COMPUTE THE SMOOTHING MATRIX, BASED ON THE **** 
**** EQUATION: -1 **** 

**** a(K) = P (K j K) *TRANS { PHI ) *P ( K+l J K) **** 

CALL TRANS (PHI, N,N, PHIT,ND,ND) 

CALL RECIP(PKKP1, TEMPI, N,ND) 

CALL PROD (PHIT, TEMPI, N,N,N,TEMP2,ND,ND,ND) 

CALL PROD ( PKKS , TEMP2 , N , N , N , AK , ND , ND , ND) 
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c ****** SOLVE FOR SMOOTHED ESTIMATE **** 

C ****** XS (K) = X (K ] K) + A(K) * [XS (K+l) - X(K+l|K)] **** 

C 

DO 504 I = 1,9 

XP1(I) = XKKS (I , KI) 

XNNMl(I) = XKKS (I , KI+1) 

504 CONTINUE 

CALL PROD ( PHI , XP1 , N , N , ONE , TMPV1 , ND , ND , ONE ) 

CALL SUB ( XNNM1 , TMPV1 , N , ONE , TMPV2 , ND , ONE) 

CALL PROD ( AK , TMPV2 , N, N, ONE , TMPV1 , ND , ND , ONE ) 

CALL ADD ( XP1 , TMPV1 , N , ONE , TMPV2 , ND , ONE ) 

DO 505 I = 1,9 

XKKS ( I , KI ) = TMPV2 ( I ) 

505 CONTINUE 
C 

C ***** AND UPDATE THE SMOOTHED COVARIANCE OF ERROR **** 
C ***** ESTIMATE MATRIX BASED ON THE EQUATION **** 

C 

C ***** PS(KjK) = P(KjK) + **** 

C ***** A ( K ) * [ PS ( K+ 1 j K+ 1 ) - P (K+l j K) ] *TRANS { A (K) ) **** 

C 

CALL SUB ( PKKS , PKKP1 ,N,N, TEMPI , ND , ND) 

CALL TRANS ( AK, N, N , TEMP2 , ND, ND) 

CALL PROD ( TEMP 1 , TEMP2 ,N,N,N, TEMP3 , ND , ND , ND ) 

CALL PROD (AK, TEMP3 ,N,N,N, TEMPI, ND,ND,ND) 

CALL ADD ( PKKS , TEMPI ,N,N, PKKS , ND , ND) 

C 

C ***** AND CONTINUE UNTIL ALL RECORDS ARE SMOOTHED **** 
C 600 CONTINUE 
C 

C ***** SAVE THE SMOOTHED DATA **** 

C 

DO 601 KI = 1 , IR1 

WRITE (3, 602) VTIME(KI) ,XKKS(1,KI) ,XKKS(4,KI) , 

* XKKS ( 7 , KI ) 

601 CONTINUE 

***** CLOSE ALL FILES AND EXIT **** 

CLOSE (3) 

CLOSE (63 , STATUS= ' DELETE ' ) 

CLOSE (61, STATUS= ' DELETE ' ) 

C 

STOP 

END 

C 

C 

C*********************** SECTION 6 ************************* 
( 2 ********************************************************** 
C ** REQUIRED SUBROUTINES ** 

Q ********************************************************** 
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********************************************************** 
SUBROUTINE WHICH COMPUTES THE PHI MATRIX 
THE MATRIX IS BUILD OF SUCCESSIVE BLOCKS, ONE 
PER COMPONENT DIRECTION OBSERVED, WHERE EACH 
BLOCK HAS THE FORM: 

j 1 T T^2/2 j 

0 1 T 

0 0 1 

THIS PROCEDURE ASSUMES THE COMPONENTS OF THE "RAW" 

STATE VECTOR ARE T 

[ X X' X" Y Y' Y" Z Z' Z" ] 

******************************************** 



SUBROUTINE PHIDEL(T , NW, PHI , ND) 
DOUBLE PRECISION PHI (ND, ND) 



IF ( 3 *NW .GT. ND ) THEN 
WRITE ( * , 201) NW, ND 
STOP 
ENDIF 



DO 100 IR = 1 , NW 

IBLK = 3 * ( IR-1) 

PHI ( IBLK+1, IBLK+1) 
PHI (IBLK+1, IBLK+2) 
PHI (IBLK+1, IBLK+3) 
PHI (IBLK+2 , IBLK+2 ) 
PHI (IBLK+2, IBLK+3) 
100 PHI (IBLK+3, IBLK+3) 

RETURN 



1 . 

T 

T*T/2 . DO 
1 . 

T 

1 . 



201 FORMAT ( ' **** ERROR IN SUBROUTINE PHIDEL - 3*NW 

* ( = ',13,') \ • .GT. ND (=',13,') ') 

END 



********************************************************** 

SUBROUTINE WHICH ADDS TWO INPUT MATRICES 
********************************************************** 

SUBROUTINE ADD (A, B,N,M, C,ND,MD) 

DOUBLE PRECISION A (ND, MD) , B (ND, MD) , C (ND, MD) 

IF ( ( N .GT. ND ) .OR. ( M .GT. MD ) ) THEN 

WRITE (* , 201) N, ND, M, MD 
STOP 
ENDIF 

DO 100 I = 1 , N 
DO 100 J = 1 , M 
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100 



C(I,J) = A ( I , J) + B(I , J) 

RETURN 

201 FORMAT ( ' **** ERROR IN SUBROUTINE ADD - ',/,10X, 

* 'Either N (=',13,')',' .GT. ND (=', 13 10X, 

* 'or M (=',13,')',' .GT. MD (=',13,')') 

END 

********************************************************** 
SUBROUTINE WHICH SUBTRACTS THE SECOND INPUT MATRIX FROM 

THE FIRST 

********************************************************** 



SUBROUTINE SUB ( A, B , N, M, C, ND, MD) 

DOUBLE PRECISION A (ND, MD) , B (ND, MD) , C (ND, MD) 

IF ( ( N .GT. ND ) .OR. ( M .GT. MD ) ) THEN 
WRITE ( * , 201) N, ND, M, MD 
STOP 
ENDIF 

DO 100 I = 1 , N 

DO 100 J = 1 , M 

100 C(I,J) = A ( I , J) - B ( I , J) 

RETURN 

201 FORMAT (' **** ERROR IN SUBROUTINE SUB - ',/,10X, 

* 'Either N (=',13,')',' .GT. ND (=', 13 ,')',/, 10X, 

* 'or M (=',13,')',' .GT. MD (=',13,')') 



END 

************************************************* 

SUBROUTINE WHICH MULTIPLIES TWO INPUT MATRICES 
************************************************* 

SUBROUTINE PROD (A , B , N , M , L , C , ND , MD , LD) 

DOUBLE PRECISION A (ND, MD) , B (MD , LD) , C (ND, LD) 

IF ( ( N .GT. ND ) .OR. ( M .GT. MD ) .OR. 

* ( L .GT. LD ) ) THEN 

WRITE (* , 201) N, ND, M, MD, L, LD 
STOP 
ENDIF 
C 

DO 100 I = 1 , N 
DO 100 J = 1, L 
C(I,J) = 0 . DO 
DO 100 K = 1 , M 

C(I,J) =C(I,J) + A(I,K)*B(K,J) 
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100 CONTINUE 
RETURN 



201 FORMAT (' **** ERROR IN SUBROUTINE PROD 



* 


'Either N 


(=\I3, 


')',' .GT. 


ND 


( = 


* 


•„or M 


(=\I3, 


' ) ' , ' . GT . 


MD 


( = 


* 


r or L 


(=\I3, 


' ) ' , ' .GT. 


LD 


( = 



- ',/,iox, 

13, ') ' , / , 10X , 
13, ’) \/,10X, 
13, ') ') 



END 

C 

Q ********************************************************** 

C SUBROUTINE WHICH COMPUTES THE TRANSPOSE OF THE INPUT 
C MATRIX 

Q ********************************************************** 

c 

SUBROUTINE TRANS (A, N , M, C , ND, MD) 

DOUBLE PRECISION A (ND, MD) , C (MD, ND) 

C 

IF ( ( N .GT. ND) .OR. ( M .GT. MD) ) THEN 
WRITE ( * , 201) N, ND, M, MD 
STOP 
ENDIF 
C 

DO 100 I = 1 , N 

DO -100 J = 1,M 

C ( J , I ) = A ( I , J) 

100 CONTINUE 
RETURN 
C 

201 FORMAT (' **** ERROR IN SUBROUTINE TRANS - ' ,/,10X, 

* 'Either N (=',13,')',' .GT. ND (=', 13 ,')',/, 10X, 

* 'or M (=',13,')',' .GT. MD (=',13,')') 

C 

END 

C 

Q ********************************************************* 

C SUBROUTINE WHICH CALCULATES THE INVERSE OF THE INPUT 
C MATRIX 

C ********************************************************* 

c 

SUBROUTINE RECIP(A, C, N, ND) 

DOUBLE PRECISION A (ND, ND) , C (ND, ND) , D ( 20 , 20) , TEMP 
C 

IF ( N .GT. ND ) THEN 
WRITE ( * , 801) N, ND 
STOP 
ENDIF 
C 

DO 100 I = 1 , N 
DO 100 J = 1 , N 
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100 D ( I , J) = A ( I , J) 

C 

DO 115 I = 1 , N 
DO 115 J = N+l , 2 *N 
115 D(I , J) = 0.0 
C 

DO 120 I = 1 , N 
J = I + N 
120 D(I,J) = 1.0 
C 

DO 240 K = 1 , N 
M = K + 1 

IF (K .EQ. N) GOTO 180 
L = K 

DO 140 I = M, N 

140 IF ( ABS ( D ( I , K) ) .GT. ABS (D(L, K) ) ) L= I 
IF (L .EQ. K) GOTO 180 
DO 160 J = K, 2*N 
TEMP = D(K,J) 

D(K, J) = D (L, J) 

160 D(L, J) = TEMP 

180 DO 185 J = M, 2*N 

185 D(K, J) = D(K, J)/D(K,K) 

IF (K .EQ. 1) GOTO 220 
Ml = K - 1 

DO 200 I = 1 , Ml 

DO 200 J = M, 2 *N 

200 D(I,J) = D( I , J) - D ( I , K) *D (K, J) 

C 

IF (K .EQ. N) GOTO 260 
220 DO 240 I =M,N 

DO 240 J = M, 2*N 

240 D(I,J) = D(I , J) - D ( I , K) *D (K, J) 

260 DO 265 I = 1,N 
DO 265 J = 1 , N 
K = J + N 

265 C(I,J) = D ( I , K) 

RETURN 

C 

801 FORMAT ( ' **** ERROR IN SUBROUTINE RECIP - N 

*( = M3, ') ', • .GT. ND (=',13, ') ') 

END 

C 

C 

C 

C 

C ********************************************************** 
C SUBROUTINE WHICH ALLOWS THE USER TO CHANGE W AND PKKM1 
C TO ALTER THE FILTER BEHAVIOR WITHOUT HAVING TO RECOMPILE 
C THE PROGRAM 

C ********************************************************** 
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SUBROUTINE CHANGE (PKKM1 , WTMIN, WTMAX , ND) 

DOUBLE PRECISION PKKM1 (ND, ND) 

REAL* 8 X, WTMIN, WTMAX 
C 

WRITE (* , 21) PKKM1 (1,1) 

WRITE (* , 22 ) 

READ (*,23, END=2 4 ) X 
IF ( X .LE. 0. ) GOTO 24 
PKKM1 (1,1) = X 
WRITE (* , 25) PKKM1 (1,1) 

24 CONTINUE 

DO 26 1=1,9 

PKKM1 (1,1) =PKKM1 (1,1) 

26 CONTINUE 
C 

21 FORMAT (/, ' The Covariance of Error Estimate Matrix - 

* ' , 'currently has the value lOx, ' P (I , I ) = 

* ',F14.4,//,' A large value (approx. 10**6) will 

* produce ','a fast initial response.',/, 

* 'Decreasing one or more decades from this value 

* will ','slow the initial response') 

22 FORMAT (' You may simply hit "Enter" to accept this 

* value,',', or',/,' enter a new value now (being 

* sure to include the decimal point) ' ) 

23 FORMAT (D14. 5) 

25 FORMAT (' Covariance of Error Estimate Matrix reset 

* to' , ' - ' , D14 . 5 ) 

C 

WRITE (*,900) WTMIN 
WRITE (* , 22 ) 

READ( * , 23 , END=910) X 
IF ( X .LE. 0. ) GOTO 910 
WTMIN = X 
WRITE ( * , 901) WTMIN 

900 FORMAT (/, ' The current MINIMUM weight on the Q matrix 

* for a ', 'maneuver is: ',F9.3) 

901 FORMAT (' MINIMUM weight on the Q matrix for a ', 

* 'maneuver reset to: ',F9.3) 

C 

910 WRITE ( * , 905) WTMAX 
WRITE (*,22) 

READ (*,23, END=920 ) X 
IF ( X .LE. 0 ) GOTO 920 
WTMAX = X 

WRITE (*,906) WTMAX 
920 CONTINUE 

905 FORMAT (/ , ' The current MAXIMUM weight on the Q matrix 

* for a ', 'maneuver is: ' ,F9.3) 

906 FORMAT (' MAXIMUM weight on the Q matrix for a ', 

* 'maneuver reset to: ',F9.3) 



73 



c 

RETURN 

END 

C 

C 

Q ********************************************************** 

C SUBROUTINE WHICH ALLOWS USER TO CHANGE THRESHOLDS 
0 ********************************************************** 
c 

SUBROUT I NE S ETTHRS H ( ATHRS H , RTHRS H ) 

REAL* 8 ATHRSH, RTHRS H, TMP 
C 

WRITE (*,10) ATHRSH 
WRITE (*, 22) 

READ (*,50, END=2 5 ) TMP 
IF ( TMP .LE. 0. ) GOTO 25 

ATHRSH = TMP 
WRITE (*,11) ATHRSH 

10 FORMAT (/,' The current Acceleration Threshold 

* is: 1 , F8 . 3 ) 

11 FORMAT (' Acceleration Threshold reset to:',F8.3) 

22 FORMAT (' You may simply hit "Enter" to accept this 

* value, ' , 

* ', or',/,' enter a new value now (being sure to 

* include the decimal point)') 

50 FORMAT (D12. 3) 

C 

25 WRITE (*,30) RTHRS H 
WRITE ( * , 22 ) 

READ (*,50, END=4 0 ) TMP 
IF ( TMP .LE. 0.0 ) GOTO 40 
RTHRS H = TMP 
WRITE (*,31) RTHRS H 

30 FORMAT (/, ' The current Roll Threshold is:',F8.3) 

31 FORMAT (' Roll Threshold reset to:',F8.3) 

C 

40 CONTINUE 
RETURN 
END 
C 
C 

c ********************************************************** 
C SUBROUTINE TO DETECT IF A MANEUVER IS OCCURRING AND SET Q 
C APPROPRIATELY. THIS SUBROUTINE ASSUMES THE COMPONENTS OF 
C THE "RAW" STATE VECTOR ARE 
C 

C T 



c 

r • 




[ X X' 


X" 


Y Y' Y" 


Z Z' 


Z" ] 


c 

c 


AND 

THE 


BUILDS THE 
PATTERN : 


Q 


MATRIX 


BLOCK 


BY BLOCK, ACCORDING TO 
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Q ( 1 , 1) = (DT**6/36) * WT 
Q ( 2 , 2 ) = (DT**4/4) * WT 

Q ( 3 , 3 ) = DT**2 * WT 



BUT CHANGING THE WEIGHTS IN EACH BLOCK ACCORDING TO 
WHETHER THE TORPEDO APPEARS TO BE MANEUVERING IN THAT 
DIRECTION. 

*★*****★************★★★★★★★*******★★★*★**★★★*****★★★★***** 



SUBROUTINE MANDET ( RL , ZK , K , RTH , ATH , WTMIN , WTMAX , 
* DT,Q,ND,NW) 



DOUBLE PRECISION RL( 1) , ZK (ND, 1) , Q (ND, ND) 

DOUBLE PRECISION RTH , ATH , WTMIN , WTMAX , WT 
INTEGER K, ND, NW 

IF ( 3*NW .GT. ND ) THEN 
WRITE ( * , 201) 3*NW, ND 

STOP 
ENDIF 

DO 100 ICOM=l , NW 
WT=WTMIN 

IF ( ( ABS (RL(K) ) .GT.RTH ) .OR. 

* ( ABS ( ZK ( 3 *ICOM, K) ) .GT. ATH ) ) WT=WTMAX 

IBLK = 3 *ICOM 

Q ( IBLK-2 , IBLK-2 ) = (DT**6) *WT/36 . DO 
Q (IBLK-1 , IBLK-1 ) = (DT**4 ) *WT/4 . DO 
Q(IBLK,IBLK) = ( DT**2 ) *WT 

100 CONTINUE 
RETURN 

201 FORMAT ( ' **** ERROR IN SUBROUTINE MANDET - 3*NW 

* (=',13,')',' .GT. ND (= 1 , 13 , ' ) ' ) 

END 



********************************************************** 
SUBROUTINE TO BUILD THE RANGE DEPENDENT COVARIANCE OF 
MEASUREMENT MATRIX - R (TBD) 

THIS SUBROUTINE ASSUMES THE COMPONENTS OF THE "RAW" 

STATE VECTOR ARE T 

[ X X' X" Y Y' Y" Z Z' Z" ] 

********************************************************** 

SUBROUTINE BUILDR(R, NW, ND) 

DOUBLE PRECISION R(ND, ND) 

INTEGER NW 
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IF ( 3 *NW .GT. ND ) THEN 
WRITE ( * , 201) 3 *NW , ND 

STOP 
ENDIF 

***** ZERO OUT ANY PREVIOUS VALUES ******* 

DO 90 IVAR = 1 , ND 

DO 90 JVAR = 1 , ND 

R( IVAR, JVAR) = 0 . DO 
90 CONTINUE 

DO 100 IVAR = 1, NW 

R ( 3 *IVAR-2 , 3 *IVAR-2 ) = 15.0D0 
R ( 3*IVAR-1 , 3*IVAR-1) = .03D0 
R ( 3* IVAR, 3*IVAR) = .09D0 

100 CONTINUE 
RETURN 

201 FORMAT (' **** ERROR IN SUBROUTINE BUILDR - 3*NW 

* (=' ,13, ') \ ' .GT. ND (=' ,13, ') ') 

END 

********************************************************** 
SUBROUTINE TO BUILD THE MEASUREMENT MATRIX - H 

THIS SUBROUTINE ASSUMES THE COMPONENTS OF THE "RAW" 

STATE VECTOR ARE T 

[ XX' X" Y Y' Y" Z Z' Z" ] 

SRC IS INTERPRETED AS A BINARY NUMBER, WHERE A "1" IN 
A GIVEN POSITION MEANS THAT COMPONENT IS ALSO PRESENT IN 
THE CORRESPONDING OBSERVATION, E.G. 

SRC = 23 = (000010111) (binary) 

IMPLIES X(l) ,X(2) ,X(3) AND X(5) MAKE UP THE OBSERVATION. 
********************************************************** 

SUBROUTINE BUILDH (SRC , M, H , NW, ND) 

DOUBLE PRECISION H (ND, ND) 

INTEGER NW , ND , SRC , M , IROW 

INTEGER BINCODE (9 ) /I , 2 , 4 , 8 , 16 , 32 , 64 , 128 , 256/ 

IF ( ( 3*NW .GT. ND ) .OR. ( ND .GT. 9 ) ) THEN 

WRITE ( * , 201) 3*NW, ND 

STOP 
ENDIF 

***** ZERO OUT ANY PREVIOUS VALUES ******* 



DO 90 IVAR=1 , ND 
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c 



DO 90 JVAR=1,ND 
H ( I VAR , JVAR) 
90 CONTINUE 

IROW = 0 



0 . DO 



******* INCLUDE observations as appropriate ***** 
******* based on whether the corresponding ***** 
******* binary digit is present IN SRC ***** 

DO 100 IVAR=1 , ND 

IF ( MOD ( SRC/BINCODE (IVAR) ,2) .EQ. 1 ) THEN 
' IROW = IROW + 1 

H ( IROW, IVAR ) = 1 . DO 
ENDIF 

100 CONTINUE 



C 

M = IROW 
RETURN 
C 

201 FORMAT ( 1 **** ERROR IN SUBROUTINE BUILDH - 3*NW 

* (= ' , 13 , * ) * , ' .GT. ND (=',13,'), OR ND > 9') 

END 

A Z 
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